<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvsfm/laSBA/sba-1.6/sba_levmar.c</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00002"></a>00002 
<a name="l00003"></a>00003 
<a name="l00004"></a>00004 
<a name="l00005"></a>00005 
<a name="l00006"></a>00006 
<a name="l00007"></a>00007 
<a name="l00008"></a>00008 
<a name="l00009"></a>00009 
<a name="l00010"></a>00010 
<a name="l00011"></a>00011 
<a name="l00012"></a>00012 
<a name="l00013"></a>00013 
<a name="l00014"></a>00014 
<a name="l00015"></a>00015 
<a name="l00016"></a>00016 
<a name="l00017"></a>00017 
<a name="l00018"></a>00018 
<a name="l00019"></a>00019 
<a name="l00020"></a>00020 
<a name="l00021"></a>00021 <span class="preprocessor">#include &lt;stdio.h&gt;</span>
<a name="l00022"></a>00022 <span class="preprocessor">#include &lt;stdlib.h&gt;</span>
<a name="l00023"></a>00023 <span class="preprocessor">#include &lt;string.h&gt;</span>
<a name="l00024"></a>00024 <span class="preprocessor">#include &lt;math.h&gt;</span>
<a name="l00025"></a>00025 <span class="preprocessor">#include &lt;float.h&gt;</span>
<a name="l00026"></a>00026 
<a name="l00027"></a>00027 <span class="preprocessor">#include &quot;compiler.h&quot;</span>
<a name="l00028"></a>00028 <span class="preprocessor">#include &quot;sba.h&quot;</span>
<a name="l00029"></a>00029 <span class="preprocessor">#include &quot;sba_chkjac.h&quot;</span>
<a name="l00030"></a>00030 
<a name="l00031"></a>00031 <span class="preprocessor">#define SBA_EPSILON       1E-12</span>
<a name="l00032"></a>00032 <span class="preprocessor"></span><span class="preprocessor">#define SBA_EPSILON_SQ    ( (SBA_EPSILON)*(SBA_EPSILON) )</span>
<a name="l00033"></a>00033 <span class="preprocessor"></span>
<a name="l00034"></a>00034 <span class="preprocessor">#define SBA_ONE_THIRD     0.3333333334 </span><span class="comment">/* 1.0/3.0 */</span>
<a name="l00035"></a>00035 
<a name="l00036"></a>00036 
<a name="l00037"></a>00037 <span class="preprocessor">#define emalloc(sz)       emalloc_(__FILE__, __LINE__, sz)</span>
<a name="l00038"></a>00038 <span class="preprocessor"></span>
<a name="l00039"></a>00039 <span class="preprocessor">#define FABS(x)           (((x)&gt;=0)? (x) : -(x))</span>
<a name="l00040"></a>00040 <span class="preprocessor"></span>
<a name="l00041"></a>00041 <span class="preprocessor">#define ROW_MAJOR         0</span>
<a name="l00042"></a>00042 <span class="preprocessor"></span><span class="preprocessor">#define COLUMN_MAJOR      1</span>
<a name="l00043"></a>00043 <span class="preprocessor"></span><span class="preprocessor">#define MAT_STORAGE       COLUMN_MAJOR</span>
<a name="l00044"></a>00044 <span class="preprocessor"></span>
<a name="l00045"></a>00045 
<a name="l00046"></a>00046 <span class="comment">/* contains information necessary for computing a finite difference approximation to a jacobian,</span>
<a name="l00047"></a>00047 <span class="comment"> * e.g. function to differentiate, problem dimensions and pointers to working memory buffers</span>
<a name="l00048"></a>00048 <span class="comment"> */</span>
<a name="l00049"></a>00049 <span class="keyword">struct </span>fdj_data_x_ {
<a name="l00050"></a>00050   void (*func)(<span class="keywordtype">double</span> *p, <span class="keyword">struct </span>sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata); <span class="comment">/* function to differentiate */</span>
<a name="l00051"></a>00051   <span class="keywordtype">int</span> cnp, pnp, mnp;  <span class="comment">/* parameter numbers */</span>
<a name="l00052"></a>00052   <span class="keywordtype">int</span> *func_rcidxs,
<a name="l00053"></a>00053       *func_rcsubs;   <span class="comment">/* working memory for func invocations.</span>
<a name="l00054"></a>00054 <span class="comment">                       * Notice that this has to be different</span>
<a name="l00055"></a>00055 <span class="comment">                       * than the working memory used for</span>
<a name="l00056"></a>00056 <span class="comment">                       * evaluating the jacobian!</span>
<a name="l00057"></a>00057 <span class="comment">                       */</span>
<a name="l00058"></a>00058   <span class="keywordtype">double</span> *hx, *hxx;   <span class="comment">/* memory to save results in */</span>
<a name="l00059"></a>00059   <span class="keywordtype">void</span> *adata;
<a name="l00060"></a>00060 };
<a name="l00061"></a>00061 
<a name="l00062"></a>00062 <span class="comment">/* auxiliary memory allocation routine with error checking */</span>
<a name="l00063"></a>00063 <span class="keyword">inline</span> <span class="keyword">static</span> <span class="keywordtype">void</span> *emalloc_(<span class="keywordtype">char</span> *file, <span class="keywordtype">int</span> line, <span class="keywordtype">size_t</span> sz)
<a name="l00064"></a>00064 {
<a name="l00065"></a>00065 <span class="keywordtype">void</span> *ptr;
<a name="l00066"></a>00066 
<a name="l00067"></a>00067   ptr=(<span class="keywordtype">void</span> *)malloc(sz);
<a name="l00068"></a>00068   <span class="keywordflow">if</span>(ptr==NULL){
<a name="l00069"></a>00069     fprintf(stderr, <span class="stringliteral">&quot;SBA: memory allocation request for %u bytes failed in file %s, line %d, exiting&quot;</span>, sz, file, line);
<a name="l00070"></a>00070     exit(1);
<a name="l00071"></a>00071   }
<a name="l00072"></a>00072 
<a name="l00073"></a>00073   <span class="keywordflow">return</span> ptr;
<a name="l00074"></a>00074 }
<a name="l00075"></a>00075 
<a name="l00076"></a>00076 <span class="comment">/* auxiliary routine for clearing an array of doubles */</span>
<a name="l00077"></a>00077 <span class="keyword">inline</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _dblzero(<span class="keyword">register</span> <span class="keywordtype">double</span> *arr, <span class="keyword">register</span> <span class="keywordtype">int</span> count)
<a name="l00078"></a>00078 {
<a name="l00079"></a>00079   <span class="keywordflow">while</span>(--count&gt;=0)
<a name="l00080"></a>00080     *arr++=0.0;
<a name="l00081"></a>00081 }
<a name="l00082"></a>00082 
<a name="l00083"></a>00083 <span class="comment">/* auxiliary routine for computing the mean reprojection error; used for debugging */</span>
<a name="l00084"></a>00084 <span class="keyword">static</span> <span class="keywordtype">double</span> sba_mean_repr_error(<span class="keywordtype">int</span> n, <span class="keywordtype">int</span> mnp, <span class="keywordtype">double</span> *x, <span class="keywordtype">double</span> *hx, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs)
<a name="l00085"></a>00085 {
<a name="l00086"></a>00086 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00087"></a>00087 <span class="keywordtype">int</span> nnz, nprojs;
<a name="l00088"></a>00088 <span class="keywordtype">double</span> *ptr1, *ptr2;
<a name="l00089"></a>00089 <span class="keywordtype">double</span> err;
<a name="l00090"></a>00090 
<a name="l00091"></a>00091   <span class="keywordflow">for</span>(i=nprojs=0, err=0.0; i&lt;n; ++i){
<a name="l00092"></a>00092     nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero x_ij, j=0...m-1 */</span>
<a name="l00093"></a>00093     nprojs+=nnz;
<a name="l00094"></a>00094     <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){ <span class="comment">/* point i projecting on camera rcsubs[j] */</span>
<a name="l00095"></a>00095       ptr1=x + idxij-&gt;val[rcidxs[j]]*mnp;
<a name="l00096"></a>00096       ptr2=hx + idxij-&gt;val[rcidxs[j]]*mnp;
<a name="l00097"></a>00097 
<a name="l00098"></a>00098       err+=sqrt((ptr1[0]-ptr2[0])*(ptr1[0]-ptr2[0]) + (ptr1[1]-ptr2[1])*(ptr1[1]-ptr2[1]));
<a name="l00099"></a>00099     }
<a name="l00100"></a>00100   }
<a name="l00101"></a>00101 
<a name="l00102"></a>00102   <span class="keywordflow">return</span> err/((double)(nprojs));
<a name="l00103"></a>00103 }
<a name="l00104"></a>00104 
<a name="l00105"></a>00105 <span class="comment">/* print the solution in p using sba&#39;s text format. If cnp/pnp==0 only points/cameras are printed */</span>
<a name="l00106"></a>00106 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_print_sol(<span class="keywordtype">int</span> n, <span class="keywordtype">int</span> m, <span class="keywordtype">double</span> *p, <span class="keywordtype">int</span> cnp, <span class="keywordtype">int</span> pnp, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> mnp, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs)
<a name="l00107"></a>00107 {
<a name="l00108"></a>00108 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii;
<a name="l00109"></a>00109 <span class="keywordtype">int</span> nnz;
<a name="l00110"></a>00110 <span class="keywordtype">double</span> *ptr;
<a name="l00111"></a>00111 
<a name="l00112"></a>00112   <span class="keywordflow">if</span>(cnp){
<a name="l00113"></a>00113     <span class="comment">/* print camera parameters */</span>
<a name="l00114"></a>00114     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00115"></a>00115       ptr=p+cnp*j;
<a name="l00116"></a>00116       <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii)
<a name="l00117"></a>00117         printf(<span class="stringliteral">&quot;%g &quot;</span>, ptr[ii]);
<a name="l00118"></a>00118       printf(<span class="stringliteral">&quot;\n&quot;</span>);
<a name="l00119"></a>00119     }
<a name="l00120"></a>00120   }
<a name="l00121"></a>00121 
<a name="l00122"></a>00122   <span class="keywordflow">if</span>(pnp){
<a name="l00123"></a>00123     <span class="comment">/* 3D &amp; 2D point parameters */</span>
<a name="l00124"></a>00124     printf(<span class="stringliteral">&quot;\n\n\n# X Y Z  nframes  frame0 x0 y0  frame1 x1 y1 ...\n&quot;</span>);
<a name="l00125"></a>00125     <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l00126"></a>00126       ptr=p+cnp*m+i*pnp;
<a name="l00127"></a>00127       <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii) <span class="comment">// print 3D coordinates</span>
<a name="l00128"></a>00128         printf(<span class="stringliteral">&quot;%g &quot;</span>, ptr[ii]);
<a name="l00129"></a>00129 
<a name="l00130"></a>00130       nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero x_ij, j=0...m-1 */</span>
<a name="l00131"></a>00131       printf(<span class="stringliteral">&quot;%d &quot;</span>, nnz);
<a name="l00132"></a>00132 
<a name="l00133"></a>00133       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){ <span class="comment">/* point i projecting on camera rcsubs[j] */</span>
<a name="l00134"></a>00134         ptr=x + idxij-&gt;val[rcidxs[j]]*mnp;
<a name="l00135"></a>00135 
<a name="l00136"></a>00136         printf(<span class="stringliteral">&quot;%d &quot;</span>, rcsubs[j]);
<a name="l00137"></a>00137         <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii) <span class="comment">// print 2D coordinates</span>
<a name="l00138"></a>00138           printf(<span class="stringliteral">&quot;%g &quot;</span>, ptr[ii]);
<a name="l00139"></a>00139       }
<a name="l00140"></a>00140       printf(<span class="stringliteral">&quot;\n&quot;</span>);
<a name="l00141"></a>00141     }
<a name="l00142"></a>00142   }
<a name="l00143"></a>00143 }
<a name="l00144"></a>00144 
<a name="l00145"></a>00145 <span class="comment">/* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e.</span>
<a name="l00146"></a>00146 <span class="comment"> * e can coincide with either x or y. </span>
<a name="l00147"></a>00147 <span class="comment"> * Uses loop unrolling and blocking to reduce bookkeeping overhead &amp; pipeline</span>
<a name="l00148"></a>00148 <span class="comment"> * stalls and increase instruction-level parallelism; see http://www.abarnett.demon.co.uk/tutorial.html</span>
<a name="l00149"></a>00149 <span class="comment"> */</span>
<a name="l00150"></a>00150 <span class="keyword">static</span> <span class="keywordtype">double</span> nrmL2xmy(<span class="keywordtype">double</span> *<span class="keyword">const</span> e, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> x, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> y, <span class="keyword">const</span> <span class="keywordtype">int</span> n)
<a name="l00151"></a>00151 {
<a name="l00152"></a>00152 <span class="keyword">const</span> <span class="keywordtype">int</span> blocksize=8, bpwr=3; <span class="comment">/* 8=2^3 */</span>
<a name="l00153"></a>00153 <span class="keyword">register</span> <span class="keywordtype">int</span> i;
<a name="l00154"></a>00154 <span class="keywordtype">int</span> j1, j2, j3, j4, j5, j6, j7;
<a name="l00155"></a>00155 <span class="keywordtype">int</span> blockn;
<a name="l00156"></a>00156 <span class="keyword">register</span> <span class="keywordtype">double</span> sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0;
<a name="l00157"></a>00157 
<a name="l00158"></a>00158   <span class="comment">/* n may not be divisible by blocksize, </span>
<a name="l00159"></a>00159 <span class="comment">   * go as near as we can first, then tidy up.</span>
<a name="l00160"></a>00160 <span class="comment">   */</span>
<a name="l00161"></a>00161   blockn = (n&gt;&gt;bpwr)&lt;&lt;bpwr; <span class="comment">/* (n / blocksize) * blocksize; */</span>
<a name="l00162"></a>00162 
<a name="l00163"></a>00163   <span class="comment">/* unroll the loop in blocks of `blocksize&#39;; looping downwards gains some more speed */</span>
<a name="l00164"></a>00164   <span class="keywordflow">for</span>(i=blockn-1; i&gt;0; i-=blocksize){
<a name="l00165"></a>00165             e[i ]=x[i ]-y[i ]; sum0+=e[i ]*e[i ];
<a name="l00166"></a>00166     j1=i-1; e[j1]=x[j1]-y[j1]; sum1+=e[j1]*e[j1];
<a name="l00167"></a>00167     j2=i-2; e[j2]=x[j2]-y[j2]; sum2+=e[j2]*e[j2];
<a name="l00168"></a>00168     j3=i-3; e[j3]=x[j3]-y[j3]; sum3+=e[j3]*e[j3];
<a name="l00169"></a>00169     j4=i-4; e[j4]=x[j4]-y[j4]; sum0+=e[j4]*e[j4];
<a name="l00170"></a>00170     j5=i-5; e[j5]=x[j5]-y[j5]; sum1+=e[j5]*e[j5];
<a name="l00171"></a>00171     j6=i-6; e[j6]=x[j6]-y[j6]; sum2+=e[j6]*e[j6];
<a name="l00172"></a>00172     j7=i-7; e[j7]=x[j7]-y[j7]; sum3+=e[j7]*e[j7];
<a name="l00173"></a>00173   }
<a name="l00174"></a>00174 
<a name="l00175"></a>00175   <span class="comment">/*</span>
<a name="l00176"></a>00176 <span class="comment">   * There may be some left to do.</span>
<a name="l00177"></a>00177 <span class="comment">   * This could be done as a simple for() loop, </span>
<a name="l00178"></a>00178 <span class="comment">   * but a switch is faster (and more interesting) </span>
<a name="l00179"></a>00179 <span class="comment">   */</span>
<a name="l00180"></a>00180 
<a name="l00181"></a>00181   i=blockn;
<a name="l00182"></a>00182   <span class="keywordflow">if</span>(i&lt;n){ 
<a name="l00183"></a>00183   <span class="comment">/* Jump into the case at the place that will allow</span>
<a name="l00184"></a>00184 <span class="comment">   * us to finish off the appropriate number of items. </span>
<a name="l00185"></a>00185 <span class="comment">   */</span>
<a name="l00186"></a>00186     <span class="keywordflow">switch</span>(n - i){ 
<a name="l00187"></a>00187       <span class="keywordflow">case</span> 7 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
<a name="l00188"></a>00188       <span class="keywordflow">case</span> 6 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
<a name="l00189"></a>00189       <span class="keywordflow">case</span> 5 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
<a name="l00190"></a>00190       <span class="keywordflow">case</span> 4 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
<a name="l00191"></a>00191       <span class="keywordflow">case</span> 3 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
<a name="l00192"></a>00192       <span class="keywordflow">case</span> 2 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
<a name="l00193"></a>00193       <span class="keywordflow">case</span> 1 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
<a name="l00194"></a>00194     }
<a name="l00195"></a>00195   }
<a name="l00196"></a>00196 
<a name="l00197"></a>00197   <span class="keywordflow">return</span> sum0+sum1+sum2+sum3;
<a name="l00198"></a>00198 }
<a name="l00199"></a>00199 
<a name="l00200"></a>00200 <span class="comment">/* Compute e=W*(x-y) for two n-vectors x and y and return the squared L2 norm of e.</span>
<a name="l00201"></a>00201 <span class="comment"> * This norm equals the squared C norm of x-y with C=W^T*W, W being block diagonal</span>
<a name="l00202"></a>00202 <span class="comment"> * matrix with nvis mnp*mnp blocks: e^T*e=(x-y)^T*W^T*W*(x-y)=||x-y||_C.</span>
<a name="l00203"></a>00203 <span class="comment"> * Note that n=nvis*mnp; e can coincide with either x or y.</span>
<a name="l00204"></a>00204 <span class="comment"> *</span>
<a name="l00205"></a>00205 <span class="comment"> * Similarly to nrmL2xmy() above, uses loop unrolling and blocking</span>
<a name="l00206"></a>00206 <span class="comment"> */</span>
<a name="l00207"></a>00207 <span class="keyword">static</span> <span class="keywordtype">double</span> nrmCxmy(<span class="keywordtype">double</span> *<span class="keyword">const</span> e, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> x, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> y,
<a name="l00208"></a>00208                       <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> W, <span class="keyword">const</span> <span class="keywordtype">int</span> mnp, <span class="keyword">const</span> <span class="keywordtype">int</span> nvis)
<a name="l00209"></a>00209 {
<a name="l00210"></a>00210 <span class="keyword">const</span> <span class="keywordtype">int</span> n=nvis*mnp;
<a name="l00211"></a>00211 <span class="keyword">const</span> <span class="keywordtype">int</span> blocksize=8, bpwr=3; <span class="comment">/* 8=2^3 */</span>
<a name="l00212"></a>00212 <span class="keyword">register</span> <span class="keywordtype">int</span> i, ii, k;
<a name="l00213"></a>00213 <span class="keywordtype">int</span> j1, j2, j3, j4, j5, j6, j7;
<a name="l00214"></a>00214 <span class="keywordtype">int</span> blockn, mnpsq;
<a name="l00215"></a>00215 <span class="keyword">register</span> <span class="keywordtype">double</span> norm, sum;
<a name="l00216"></a>00216 <span class="keyword">register</span> <span class="keyword">const</span> <span class="keywordtype">double</span> *Wptr, *auxptr;
<a name="l00217"></a>00217 <span class="keyword">register</span> <span class="keywordtype">double</span> *eptr;
<a name="l00218"></a>00218 
<a name="l00219"></a>00219   <span class="comment">/* n may not be divisible by blocksize, </span>
<a name="l00220"></a>00220 <span class="comment">   * go as near as we can first, then tidy up.</span>
<a name="l00221"></a>00221 <span class="comment">   */</span>
<a name="l00222"></a>00222   blockn = (n&gt;&gt;bpwr)&lt;&lt;bpwr; <span class="comment">/* (n / blocksize) * blocksize; */</span>
<a name="l00223"></a>00223 
<a name="l00224"></a>00224   <span class="comment">/* unroll the loop in blocks of `blocksize&#39;; looping downwards gains some more speed */</span>
<a name="l00225"></a>00225   <span class="keywordflow">for</span>(i=blockn-1; i&gt;0; i-=blocksize){
<a name="l00226"></a>00226             e[i ]=x[i ]-y[i ];
<a name="l00227"></a>00227     j1=i-1; e[j1]=x[j1]-y[j1];
<a name="l00228"></a>00228     j2=i-2; e[j2]=x[j2]-y[j2];
<a name="l00229"></a>00229     j3=i-3; e[j3]=x[j3]-y[j3];
<a name="l00230"></a>00230     j4=i-4; e[j4]=x[j4]-y[j4];
<a name="l00231"></a>00231     j5=i-5; e[j5]=x[j5]-y[j5];
<a name="l00232"></a>00232     j6=i-6; e[j6]=x[j6]-y[j6];
<a name="l00233"></a>00233     j7=i-7; e[j7]=x[j7]-y[j7];
<a name="l00234"></a>00234   }
<a name="l00235"></a>00235 
<a name="l00236"></a>00236   <span class="comment">/*</span>
<a name="l00237"></a>00237 <span class="comment">   * There may be some left to do.</span>
<a name="l00238"></a>00238 <span class="comment">   * This could be done as a simple for() loop, </span>
<a name="l00239"></a>00239 <span class="comment">   * but a switch is faster (and more interesting) </span>
<a name="l00240"></a>00240 <span class="comment">   */</span>
<a name="l00241"></a>00241 
<a name="l00242"></a>00242   i=blockn;
<a name="l00243"></a>00243   <span class="keywordflow">if</span>(i&lt;n){ 
<a name="l00244"></a>00244   <span class="comment">/* Jump into the case at the place that will allow</span>
<a name="l00245"></a>00245 <span class="comment">   * us to finish off the appropriate number of items. </span>
<a name="l00246"></a>00246 <span class="comment">   */</span>
<a name="l00247"></a>00247     <span class="keywordflow">switch</span>(n - i){ 
<a name="l00248"></a>00248       <span class="keywordflow">case</span> 7 : e[i]=x[i]-y[i]; ++i;
<a name="l00249"></a>00249       <span class="keywordflow">case</span> 6 : e[i]=x[i]-y[i]; ++i;
<a name="l00250"></a>00250       <span class="keywordflow">case</span> 5 : e[i]=x[i]-y[i]; ++i;
<a name="l00251"></a>00251       <span class="keywordflow">case</span> 4 : e[i]=x[i]-y[i]; ++i;
<a name="l00252"></a>00252       <span class="keywordflow">case</span> 3 : e[i]=x[i]-y[i]; ++i;
<a name="l00253"></a>00253       <span class="keywordflow">case</span> 2 : e[i]=x[i]-y[i]; ++i;
<a name="l00254"></a>00254       <span class="keywordflow">case</span> 1 : e[i]=x[i]-y[i]; ++i;
<a name="l00255"></a>00255     }
<a name="l00256"></a>00256   }
<a name="l00257"></a>00257 
<a name="l00258"></a>00258   <span class="comment">/* compute w_x_ij e_ij in e and its L2 norm.</span>
<a name="l00259"></a>00259 <span class="comment">   * Since w_x_ij is upper triangular, the products can be safely saved</span>
<a name="l00260"></a>00260 <span class="comment">   * directly in e_ij, without the need for intermediate storage</span>
<a name="l00261"></a>00261 <span class="comment">   */</span>
<a name="l00262"></a>00262   mnpsq=mnp*mnp;
<a name="l00263"></a>00263   <span class="comment">/* Wptr, eptr point to w_x_ij, e_ij below */</span>
<a name="l00264"></a>00264   <span class="keywordflow">for</span>(i=0, Wptr=W, eptr=e, norm=0.0; i&lt;nvis; ++i, Wptr+=mnpsq, eptr+=mnp){
<a name="l00265"></a>00265     <span class="keywordflow">for</span>(ii=0, auxptr=Wptr; ii&lt;mnp; ++ii, auxptr+=mnp){ <span class="comment">/* auxptr=Wptr+ii*mnp */</span>
<a name="l00266"></a>00266       <span class="keywordflow">for</span>(k=ii, sum=0.0; k&lt;mnp; ++k) <span class="comment">// k&gt;=ii since w_x_ij is upper triangular</span>
<a name="l00267"></a>00267         sum+=auxptr[k]*eptr[k]; <span class="comment">//Wptr[ii*mnp+k]*eptr[k];</span>
<a name="l00268"></a>00268       eptr[ii]=sum;
<a name="l00269"></a>00269       norm+=sum*sum;
<a name="l00270"></a>00270     }
<a name="l00271"></a>00271   }
<a name="l00272"></a>00272 
<a name="l00273"></a>00273   <span class="keywordflow">return</span> norm;
<a name="l00274"></a>00274 }
<a name="l00275"></a>00275 
<a name="l00276"></a>00276 <span class="comment">/* search for &amp; print image projection components that are infinite; useful for identifying errors */</span>
<a name="l00277"></a>00277 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_print_inf(<span class="keywordtype">double</span> *hx, <span class="keywordtype">int</span> nimgs, <span class="keywordtype">int</span> mnp, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs)
<a name="l00278"></a>00278 {
<a name="l00279"></a>00279 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, k;
<a name="l00280"></a>00280 <span class="keywordtype">int</span> nnz;
<a name="l00281"></a>00281 <span class="keywordtype">double</span> *phxij;
<a name="l00282"></a>00282 
<a name="l00283"></a>00283   <span class="keywordflow">for</span>(j=0; j&lt;nimgs; ++j){
<a name="l00284"></a>00284     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00285"></a>00285     <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00286"></a>00286       phxij=hx + idxij-&gt;val[rcidxs[i]]*mnp;
<a name="l00287"></a>00287       <span class="keywordflow">for</span>(k=0; k&lt;mnp; ++k)
<a name="l00288"></a>00288         <span class="keywordflow">if</span>(!SBA_FINITE(phxij[k]))
<a name="l00289"></a>00289           printf(<span class="stringliteral">&quot;SBA: component %d of the estimated projection of point %d on camera %d is invalid!\n&quot;</span>, k, rcsubs[i], j);
<a name="l00290"></a>00290     }
<a name="l00291"></a>00291   }
<a name="l00292"></a>00292 }
<a name="l00293"></a>00293 
<a name="l00294"></a>00294 <span class="comment">/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in</span>
<a name="l00295"></a>00295 <span class="comment"> * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.</span>
<a name="l00296"></a>00296 <span class="comment"> * The jacobian is approximated with the aid of finite differences and is returned in the order</span>
<a name="l00297"></a>00297 <span class="comment"> * (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm),</span>
<a name="l00298"></a>00298 <span class="comment"> * where A_ij=dx_ij/da_j and B_ij=dx_ij/db_i (see HZ).</span>
<a name="l00299"></a>00299 <span class="comment"> * Notice that depending on idxij, some of the A_ij, B_ij might be missing</span>
<a name="l00300"></a>00300 <span class="comment"> *</span>
<a name="l00301"></a>00301 <span class="comment"> * Problem-specific information is assumed to be stored in a structure pointed to by &quot;dat&quot;.</span>
<a name="l00302"></a>00302 <span class="comment"> *</span>
<a name="l00303"></a>00303 <span class="comment"> * NOTE: The jacobian (for n=4, m=3) in matrix form has the following structure:</span>
<a name="l00304"></a>00304 <span class="comment"> *       A_11  0     0     B_11 0    0    0</span>
<a name="l00305"></a>00305 <span class="comment"> *       0     A_12  0     B_12 0    0    0</span>
<a name="l00306"></a>00306 <span class="comment"> *       0     0     A_13  B_13 0    0    0</span>
<a name="l00307"></a>00307 <span class="comment"> *       A_21  0     0     0    B_21 0    0</span>
<a name="l00308"></a>00308 <span class="comment"> *       0     A_22  0     0    B_22 0    0</span>
<a name="l00309"></a>00309 <span class="comment"> *       0     0     A_23  0    B_23 0    0</span>
<a name="l00310"></a>00310 <span class="comment"> *       A_31  0     0     0    0    B_31 0</span>
<a name="l00311"></a>00311 <span class="comment"> *       0     A_32  0     0    0    B_32 0</span>
<a name="l00312"></a>00312 <span class="comment"> *       0     0     A_33  0    0    B_33 0</span>
<a name="l00313"></a>00313 <span class="comment"> *       A_41  0     0     0    0    0    B_41</span>
<a name="l00314"></a>00314 <span class="comment"> *       0     A_42  0     0    0    0    B_42</span>
<a name="l00315"></a>00315 <span class="comment"> *       0     0     A_43  0    0    0    B_43</span>
<a name="l00316"></a>00316 <span class="comment"> *       To reduce the total number of objective function evaluations, this structure can be</span>
<a name="l00317"></a>00317 <span class="comment"> *       exploited as follows: A certain d is added to the j-th parameters of all cameras and</span>
<a name="l00318"></a>00318 <span class="comment"> *       the objective function is evaluated at the resulting point. This evaluation suffices</span>
<a name="l00319"></a>00319 <span class="comment"> *       to compute the corresponding columns of *all* A_ij through finite differences. A similar</span>
<a name="l00320"></a>00320 <span class="comment"> *       strategy allows the computation of the B_ij. Overall, only cnp+pnp+1 objective function</span>
<a name="l00321"></a>00321 <span class="comment"> *       evaluations are needed to compute the jacobian, much fewer compared to the m*cnp+n*pnp+1</span>
<a name="l00322"></a>00322 <span class="comment"> *       that would be required by the naive strategy of computing one column of the jacobian</span>
<a name="l00323"></a>00323 <span class="comment"> *       per function evaluation. See Nocedal-Wright, ch. 7, pp. 169. Although this approach is</span>
<a name="l00324"></a>00324 <span class="comment"> *       much faster compared to the naive strategy, it is not preferable to analytic jacobians,</span>
<a name="l00325"></a>00325 <span class="comment"> *       since the latter are considerably faster to compute and result in fewer LM iterations.</span>
<a name="l00326"></a>00326 <span class="comment"> */</span>
<a name="l00327"></a>00327 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_fdjac_x(
<a name="l00328"></a>00328     <span class="keywordtype">double</span> *p,                <span class="comment">/* I: current parameter estimate, (m*cnp+n*pnp)x1 */</span>
<a name="l00329"></a>00329     <span class="keyword">struct</span> sba_crsm *idxij,   <span class="comment">/* I: sparse matrix containing the location of x_ij in hx */</span>
<a name="l00330"></a>00330     <span class="keywordtype">int</span>    *rcidxs,           <span class="comment">/* work array for the indexes of nonzero elements of a single sparse matrix row/column */</span>
<a name="l00331"></a>00331     <span class="keywordtype">int</span>    *rcsubs,           <span class="comment">/* work array for the subscripts of nonzero elements in a single sparse matrix row/column */</span>
<a name="l00332"></a>00332     <span class="keywordtype">double</span> *jac,              <span class="comment">/* O: array for storing the approximated jacobian */</span>
<a name="l00333"></a>00333     <span class="keywordtype">void</span>   *dat)              <span class="comment">/* I: points to a &quot;fdj_data_x_&quot; structure */</span>
<a name="l00334"></a>00334 {
<a name="l00335"></a>00335   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii, jj;
<a name="l00336"></a>00336   <span class="keywordtype">double</span> *pa, *pb, *pqr, *ppt;
<a name="l00337"></a>00337   <span class="keyword">register</span> <span class="keywordtype">double</span> *pAB, *phx, *phxx;
<a name="l00338"></a>00338   <span class="keywordtype">int</span> n, m, nm, nnz, Asz, Bsz, ABsz, idx;
<a name="l00339"></a>00339 
<a name="l00340"></a>00340   <span class="keywordtype">double</span> *tmpd;
<a name="l00341"></a>00341   <span class="keyword">register</span> <span class="keywordtype">double</span> d;
<a name="l00342"></a>00342 
<a name="l00343"></a>00343   <span class="keyword">struct </span>fdj_data_x_ *fdjd;
<a name="l00344"></a>00344   void (*func)(<span class="keywordtype">double</span> *p, <span class="keyword">struct </span>sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata);
<a name="l00345"></a>00345   <span class="keywordtype">double</span> *hx, *hxx;
<a name="l00346"></a>00346   <span class="keywordtype">int</span> cnp, pnp, mnp;
<a name="l00347"></a>00347   <span class="keywordtype">void</span> *adata;
<a name="l00348"></a>00348 
<a name="l00349"></a>00349 
<a name="l00350"></a>00350   <span class="comment">/* retrieve problem-specific information passed in *dat */</span>
<a name="l00351"></a>00351   fdjd=(<span class="keyword">struct </span>fdj_data_x_ *)dat;
<a name="l00352"></a>00352   func=fdjd-&gt;func;
<a name="l00353"></a>00353   cnp=fdjd-&gt;cnp; pnp=fdjd-&gt;pnp; mnp=fdjd-&gt;mnp;
<a name="l00354"></a>00354   hx=fdjd-&gt;hx;
<a name="l00355"></a>00355   hxx=fdjd-&gt;hxx;
<a name="l00356"></a>00356   adata=fdjd-&gt;adata;
<a name="l00357"></a>00357 
<a name="l00358"></a>00358   n=idxij-&gt;nr; m=idxij-&gt;nc;
<a name="l00359"></a>00359   pa=p; pb=p+m*cnp;
<a name="l00360"></a>00360   Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
<a name="l00361"></a>00361 
<a name="l00362"></a>00362   nm=(n&gt;=m)? n : m; <span class="comment">// max(n, m);</span>
<a name="l00363"></a>00363   tmpd=(<span class="keywordtype">double</span> *)emalloc(nm*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00364"></a>00364 
<a name="l00365"></a>00365   (*func)(p, idxij, fdjd-&gt;func_rcidxs, fdjd-&gt;func_rcsubs, hx, adata); <span class="comment">// evaluate supplied function on current solution</span>
<a name="l00366"></a>00366 
<a name="l00367"></a>00367   <span class="keywordflow">if</span>(cnp){ <span class="comment">// is motion varying?</span>
<a name="l00368"></a>00368     <span class="comment">/* compute A_ij */</span>
<a name="l00369"></a>00369     <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj){
<a name="l00370"></a>00370       <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00371"></a>00371         pqr=pa+j*cnp; <span class="comment">// j-th camera parameters</span>
<a name="l00372"></a>00372         <span class="comment">/* determine d=max(SBA_DELTA_SCALE*|pqr[jj]|, SBA_MIN_DELTA), see HZ */</span>
<a name="l00373"></a>00373         d=(double)(SBA_DELTA_SCALE)*pqr[jj]; <span class="comment">// force evaluation</span>
<a name="l00374"></a>00374         d=FABS(d);
<a name="l00375"></a>00375         <span class="keywordflow">if</span>(d&lt;SBA_MIN_DELTA) d=SBA_MIN_DELTA;
<a name="l00376"></a>00376 
<a name="l00377"></a>00377         tmpd[j]=d;
<a name="l00378"></a>00378         pqr[jj]+=d;
<a name="l00379"></a>00379       }
<a name="l00380"></a>00380 
<a name="l00381"></a>00381       (*func)(p, idxij, fdjd-&gt;func_rcidxs, fdjd-&gt;func_rcsubs, hxx, adata);
<a name="l00382"></a>00382 
<a name="l00383"></a>00383       <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00384"></a>00384         pqr=pa+j*cnp; <span class="comment">// j-th camera parameters</span>
<a name="l00385"></a>00385         pqr[jj]-=tmpd[j]; <span class="comment">/* restore */</span>
<a name="l00386"></a>00386         d=1.0/tmpd[j]; <span class="comment">/* invert so that divisions can be carried out faster as multiplications */</span>
<a name="l00387"></a>00387 
<a name="l00388"></a>00388         nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero A_ij, i=0...n-1 */</span>
<a name="l00389"></a>00389         <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00390"></a>00390           idx=idxij-&gt;val[rcidxs[i]];
<a name="l00391"></a>00391           phx=hx + idx*mnp; <span class="comment">// set phx to point to hx_ij</span>
<a name="l00392"></a>00392           phxx=hxx + idx*mnp; <span class="comment">// set phxx to point to hxx_ij</span>
<a name="l00393"></a>00393           pAB=jac + idx*ABsz; <span class="comment">// set pAB to point to A_ij</span>
<a name="l00394"></a>00394 
<a name="l00395"></a>00395           <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00396"></a>00396             pAB[ii*cnp+jj]=(phxx[ii]-phx[ii])*d;
<a name="l00397"></a>00397         }
<a name="l00398"></a>00398       }
<a name="l00399"></a>00399     }
<a name="l00400"></a>00400   }
<a name="l00401"></a>00401 
<a name="l00402"></a>00402   <span class="keywordflow">if</span>(pnp){ <span class="comment">// is structure varying?</span>
<a name="l00403"></a>00403     <span class="comment">/* compute B_ij */</span>
<a name="l00404"></a>00404     <span class="keywordflow">for</span>(jj=0; jj&lt;pnp; ++jj){
<a name="l00405"></a>00405       <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l00406"></a>00406         ppt=pb+i*pnp; <span class="comment">// i-th point parameters</span>
<a name="l00407"></a>00407         <span class="comment">/* determine d=max(SBA_DELTA_SCALE*|ppt[jj]|, SBA_MIN_DELTA), see HZ */</span>
<a name="l00408"></a>00408         d=(double)(SBA_DELTA_SCALE)*ppt[jj]; <span class="comment">// force evaluation</span>
<a name="l00409"></a>00409         d=FABS(d);
<a name="l00410"></a>00410         <span class="keywordflow">if</span>(d&lt;SBA_MIN_DELTA) d=SBA_MIN_DELTA;
<a name="l00411"></a>00411 
<a name="l00412"></a>00412         tmpd[i]=d;
<a name="l00413"></a>00413         ppt[jj]+=d;
<a name="l00414"></a>00414       }
<a name="l00415"></a>00415 
<a name="l00416"></a>00416       (*func)(p, idxij, fdjd-&gt;func_rcidxs, fdjd-&gt;func_rcsubs, hxx, adata);
<a name="l00417"></a>00417 
<a name="l00418"></a>00418       <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l00419"></a>00419         ppt=pb+i*pnp; <span class="comment">// i-th point parameters</span>
<a name="l00420"></a>00420         ppt[jj]-=tmpd[i]; <span class="comment">/* restore */</span>
<a name="l00421"></a>00421         d=1.0/tmpd[i]; <span class="comment">/* invert so that divisions can be carried out faster as multiplications */</span>
<a name="l00422"></a>00422 
<a name="l00423"></a>00423         nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero B_ij, j=0...m-1 */</span>
<a name="l00424"></a>00424         <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l00425"></a>00425           idx=idxij-&gt;val[rcidxs[j]];
<a name="l00426"></a>00426           phx=hx + idx*mnp; <span class="comment">// set phx to point to hx_ij</span>
<a name="l00427"></a>00427           phxx=hxx + idx*mnp; <span class="comment">// set phxx to point to hxx_ij</span>
<a name="l00428"></a>00428           pAB=jac + idx*ABsz + Asz; <span class="comment">// set pAB to point to B_ij</span>
<a name="l00429"></a>00429 
<a name="l00430"></a>00430           <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00431"></a>00431             pAB[ii*pnp+jj]=(phxx[ii]-phx[ii])*d;
<a name="l00432"></a>00432         }
<a name="l00433"></a>00433       }
<a name="l00434"></a>00434     }
<a name="l00435"></a>00435   }
<a name="l00436"></a>00436 
<a name="l00437"></a>00437   free(tmpd);
<a name="l00438"></a>00438 }
<a name="l00439"></a>00439 
<a name="l00440"></a>00440 <span class="keyword">typedef</span> int (*PLS)(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> iscolmaj);
<a name="l00441"></a>00441 
<a name="l00442"></a>00442 <span class="comment">/* Bundle adjustment on camera and structure parameters </span>
<a name="l00443"></a>00443 <span class="comment"> * using the sparse Levenberg-Marquardt as described in HZ p. 568</span>
<a name="l00444"></a>00444 <span class="comment"> *</span>
<a name="l00445"></a>00445 <span class="comment"> * Returns the number of iterations (&gt;=0) if successfull, SBA_ERROR if failed</span>
<a name="l00446"></a>00446 <span class="comment"> */</span>
<a name="l00447"></a>00447 
<a name="l00448"></a>00448 <span class="preprocessor">#ifdef SAVE_REDUCED_MATRIX</span>
<a name="l00449"></a>00449 <span class="preprocessor"></span><span class="keywordtype">double</span> *copyS = NULL;
<a name="l00450"></a>00450 <span class="keywordtype">int</span> dimS = 0;
<a name="l00451"></a>00451 <span class="preprocessor">#endif</span>
<a name="l00452"></a>00452 <span class="preprocessor"></span>
<a name="l00453"></a>00453 <span class="preprocessor">#include &lt;stdio.h&gt;</span>
<a name="l00454"></a>00454 <span class="preprocessor">#include &lt;time.h&gt;</span>
<a name="l00455"></a>00455 <span class="comment">// =====================================================================================================================================================================</span>
<a name="l00456"></a>00456 <span class="keywordtype">double</span> last_mu_used;
<a name="l00457"></a>00457 <span class="keywordtype">int</span> sba_damping_iters;
<a name="l00458"></a>00458 <span class="keywordtype">double</span> sba_time_total, sba_time_solve, sba_time_system;
<a name="l00459"></a>00459 
<a name="l00460"></a>00460 <span class="keywordtype">int</span> sba_motstr_levmar_x(
<a name="l00461"></a>00461     <span class="keyword">const</span> <span class="keywordtype">int</span> n,   <span class="comment">/* number of points */</span>
<a name="l00462"></a>00462     <span class="keyword">const</span> <span class="keywordtype">int</span> ncon,<span class="comment">/* number of points (starting from the 1st) whose parameters should not be modified.</span>
<a name="l00463"></a>00463 <span class="comment">                   * All B_ij (see below) with i&lt;ncon are assumed to be zero</span>
<a name="l00464"></a>00464 <span class="comment">                   */</span>
<a name="l00465"></a>00465     <span class="keyword">const</span> <span class="keywordtype">int</span> m,   <span class="comment">/* number of images */</span>
<a name="l00466"></a>00466     <span class="keyword">const</span> <span class="keywordtype">int</span> mcon,<span class="comment">/* number of images (starting from the 1st) whose parameters should not be modified.</span>
<a name="l00467"></a>00467 <span class="comment">                                                  * All A_ij (see below) with j&lt;mcon are assumed to be zero</span>
<a name="l00468"></a>00468 <span class="comment">                                                  */</span>
<a name="l00469"></a>00469     <span class="keywordtype">char</span> *vmask,  <span class="comment">/* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */</span>
<a name="l00470"></a>00470     <span class="keywordtype">double</span> *p,    <span class="comment">/* initial parameter vector p0: (a1, ..., am, b1, ..., bn).</span>
<a name="l00471"></a>00471 <span class="comment">                   * aj are the image j parameters, bi are the i-th point parameters,</span>
<a name="l00472"></a>00472 <span class="comment">                   * size m*cnp + n*pnp</span>
<a name="l00473"></a>00473 <span class="comment">                   */</span>
<a name="l00474"></a>00474     <span class="keyword">const</span> <span class="keywordtype">int</span> cnp,<span class="comment">/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */</span>
<a name="l00475"></a>00475     <span class="keyword">const</span> <span class="keywordtype">int</span> pnp,<span class="comment">/* number of parameters for ONE point; e.g. 3 for Euclidean points */</span>
<a name="l00476"></a>00476     <span class="keywordtype">double</span> *x,    <span class="comment">/* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where</span>
<a name="l00477"></a>00477 <span class="comment">                   * x_ij is the projection of the i-th point on the j-th image.</span>
<a name="l00478"></a>00478 <span class="comment">                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;</span>
<a name="l00479"></a>00479 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp</span>
<a name="l00480"></a>00480 <span class="comment">                   */</span>
<a name="l00481"></a>00481     <span class="keywordtype">double</span> *covx, <span class="comment">/* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm),</span>
<a name="l00482"></a>00482 <span class="comment">                   * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no</span>
<a name="l00483"></a>00483 <span class="comment">                   * covariance estimates are available (identity matrices are implicitly used in this case).</span>
<a name="l00484"></a>00484 <span class="comment">                   * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing;</span>
<a name="l00485"></a>00485 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp*mnp</span>
<a name="l00486"></a>00486 <span class="comment">                   */</span>
<a name="l00487"></a>00487     <span class="keyword">const</span> <span class="keywordtype">int</span> mnp,<span class="comment">/* number of parameters for EACH measurement; usually 2 */</span>
<a name="l00488"></a>00488     <span class="keywordtype">void</span> (*func)(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata),
<a name="l00489"></a>00489                                               <span class="comment">/* functional relation describing measurements. Given a parameter vector p,</span>
<a name="l00490"></a>00490 <span class="comment">                                               * computes a prediction of the measurements \hat{x}. p is (m*cnp + n*pnp)x1,</span>
<a name="l00491"></a>00491 <span class="comment">                                               * \hat{x} is (n*m*mnp)x1, maximum</span>
<a name="l00492"></a>00492 <span class="comment">                                               * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used</span>
<a name="l00493"></a>00493 <span class="comment">                                               * as working memory</span>
<a name="l00494"></a>00494 <span class="comment">                                               */</span>
<a name="l00495"></a>00495     <span class="keywordtype">void</span> (*fjac)(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata),
<a name="l00496"></a>00496                                               <span class="comment">/* function to evaluate the sparse jacobian dX/dp.</span>
<a name="l00497"></a>00497 <span class="comment">                                               * The Jacobian is returned in jac as</span>
<a name="l00498"></a>00498 <span class="comment">                                               * (dx_11/da_1, ..., dx_1m/da_m, ..., dx_n1/da_1, ..., dx_nm/da_m,</span>
<a name="l00499"></a>00499 <span class="comment">                                               *  dx_11/db_1, ..., dx_1m/db_1, ..., dx_n1/db_n, ..., dx_nm/db_n), or (using HZ&#39;s notation),</span>
<a name="l00500"></a>00500 <span class="comment">                                               * jac=(A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm)</span>
<a name="l00501"></a>00501 <span class="comment">                                               * Notice that depending on idxij, some of the A_ij and B_ij might be missing.</span>
<a name="l00502"></a>00502 <span class="comment">                                               * Note also that A_ij and B_ij are mnp x cnp and mnp x pnp matrices resp. and they</span>
<a name="l00503"></a>00503 <span class="comment">                                               * should be stored in jac in row-major order.</span>
<a name="l00504"></a>00504 <span class="comment">                                               * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used</span>
<a name="l00505"></a>00505 <span class="comment">                                               * as working memory</span>
<a name="l00506"></a>00506 <span class="comment">                                               *</span>
<a name="l00507"></a>00507 <span class="comment">                                               * If NULL, the jacobian is approximated by repetitive func calls and finite</span>
<a name="l00508"></a>00508 <span class="comment">                                               * differences. This is computationally inefficient and thus NOT recommended.</span>
<a name="l00509"></a>00509 <span class="comment">                                               */</span>
<a name="l00510"></a>00510     <span class="keywordtype">void</span> *adata,       <span class="comment">/* pointer to possibly additional data, passed uninterpreted to func, fjac */</span> 
<a name="l00511"></a>00511 
<a name="l00512"></a>00512     <span class="keyword">const</span> <span class="keywordtype">int</span> itmax,   <span class="comment">/* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */</span>
<a name="l00513"></a>00513     <span class="keyword">const</span> <span class="keywordtype">int</span> verbose, <span class="comment">/* I: verbosity */</span>
<a name="l00514"></a>00514     <span class="keyword">const</span> <span class="keywordtype">double</span> opts[SBA_OPTSSZ],
<a name="l00515"></a>00515                              <span class="comment">/* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu,</span>
<a name="l00516"></a>00516 <span class="comment">                        * stopping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2</span>
<a name="l00517"></a>00517 <span class="comment">                        */</span>
<a name="l00518"></a>00518     <span class="keywordtype">double</span> info[SBA_INFOSZ]
<a name="l00519"></a>00519                              <span class="comment">/* O: information regarding the minimization. Set to NULL if don&#39;t care</span>
<a name="l00520"></a>00520 <span class="comment">                        * info[0]=||e||_2 at initial p.</span>
<a name="l00521"></a>00521 <span class="comment">                        * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.</span>
<a name="l00522"></a>00522 <span class="comment">                        * info[5]= # iterations,</span>
<a name="l00523"></a>00523 <span class="comment">                        * info[6]=reason for terminating: 1 - stopped by small gradient J^T e</span>
<a name="l00524"></a>00524 <span class="comment">                        *                                 2 - stopped by small dp</span>
<a name="l00525"></a>00525 <span class="comment">                        *                                 3 - stopped by itmax</span>
<a name="l00526"></a>00526 <span class="comment">                        *                                 4 - stopped by small relative reduction in ||e||_2</span>
<a name="l00527"></a>00527 <span class="comment">                        *                                 5 - stopped by small ||e||_2</span>
<a name="l00528"></a>00528 <span class="comment">                        *                                 6 - too many attempts to increase damping. Restart with increased mu</span>
<a name="l00529"></a>00529 <span class="comment">                        *                                 7 - stopped by invalid (i.e. NaN or Inf) &quot;func&quot; values. This is a user error</span>
<a name="l00530"></a>00530 <span class="comment">                        * info[7]= # function evaluations</span>
<a name="l00531"></a>00531 <span class="comment">                        * info[8]= # jacobian evaluations</span>
<a name="l00532"></a>00532 <span class="comment">                                          * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error</span>
<a name="l00533"></a>00533 <span class="comment">                        */</span>
<a name="l00534"></a>00534 )
<a name="l00535"></a>00535 <span class="comment">// ==============================================================================================================================================================================</span>
<a name="l00536"></a>00536 {
<a name="l00537"></a>00537 <span class="comment">// ---------------------</span>
<a name="l00538"></a>00538 
<a name="l00539"></a>00539 <span class="keywordtype">int</span> time_total = 0, time_solve = 0;
<a name="l00540"></a>00540 
<a name="l00541"></a>00541 clock_t t_init = clock();
<a name="l00542"></a>00542 
<a name="l00543"></a>00543 
<a name="l00544"></a>00544 
<a name="l00545"></a>00545 <span class="comment">// ---------------------</span>
<a name="l00546"></a>00546 
<a name="l00547"></a>00547 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii, jj, k, l;
<a name="l00548"></a>00548 <span class="keywordtype">int</span> nvis, nnz, retval;
<a name="l00549"></a>00549 
<a name="l00550"></a>00550 <span class="comment">/* The following are work arrays that are dynamically allocated by sba_motstr_levmar_x() */</span>
<a name="l00551"></a>00551 <span class="keywordtype">double</span> *jac;  <span class="comment">/* work array for storing the jacobian, max. size n*m*(mnp*cnp + mnp*pnp) */</span>
<a name="l00552"></a>00552 <span class="keywordtype">double</span> *U;    <span class="comment">/* work array for storing the U_j in the order U_1, ..., U_m, size m*cnp*cnp */</span>
<a name="l00553"></a>00553 <span class="keywordtype">double</span> *V;    <span class="comment">/* work array for storing the *strictly upper triangles* of V_i in the order V_1, ..., V_n, size n*pnp*pnp.</span>
<a name="l00554"></a>00554 <span class="comment">               * V also stores the lower triangles of (V*_i)^-1 in the order (V*_1)^-1, ..., (V*_n)^-1.</span>
<a name="l00555"></a>00555 <span class="comment">               * Note that diagonal elements of V_1 are saved in diagUV</span>
<a name="l00556"></a>00556 <span class="comment">               */</span>
<a name="l00557"></a>00557 
<a name="l00558"></a>00558 <span class="keywordtype">double</span> *e;    <span class="comment">/* work array for storing the e_ij in the order e_11, ..., e_1m, ..., e_n1, ..., e_nm,</span>
<a name="l00559"></a>00559 <span class="comment">                 max. size n*m*mnp */</span>
<a name="l00560"></a>00560 <span class="keywordtype">double</span> *eab;  <span class="comment">/* work array for storing the ea_j &amp; eb_i in the order ea_1, .. ea_m eb_1, .. eb_n size m*cnp + n*pnp */</span>
<a name="l00561"></a>00561 
<a name="l00562"></a>00562 <span class="keywordtype">double</span> *E;   <span class="comment">/* work array for storing the e_j in the order e_1, .. e_m, size m*cnp */</span>
<a name="l00563"></a>00563 
<a name="l00564"></a>00564 <span class="comment">/* Notice that the blocks W_ij, Y_ij are zero iff A_ij (equivalently B_ij) is zero. This means</span>
<a name="l00565"></a>00565 <span class="comment"> * that the matrix consisting of blocks W_ij is itself sparse, similarly to the</span>
<a name="l00566"></a>00566 <span class="comment"> * block matrix made up of the A_ij and B_ij (i.e. jac)</span>
<a name="l00567"></a>00567 <span class="comment"> */</span>
<a name="l00568"></a>00568 <span class="keywordtype">double</span> *W;    <span class="comment">/* work array for storing the W_ij in the order W_11, ..., W_1m, ..., W_n1, ..., W_nm,</span>
<a name="l00569"></a>00569 <span class="comment">                 max. size n*m*cnp*pnp */</span>
<a name="l00570"></a>00570 <span class="keywordtype">double</span> *Yj;   <span class="comment">/* work array for storing the Y_ij for a *fixed* j in the order Y_1j, Y_nj,</span>
<a name="l00571"></a>00571 <span class="comment">                 max. size n*cnp*pnp */</span>
<a name="l00572"></a>00572 <span class="keywordtype">double</span> *YWt;  <span class="comment">/* work array for storing \sum_i Y_ij W_ik^T, size cnp*cnp */</span>
<a name="l00573"></a>00573 <span class="keywordtype">double</span> *S;    <span class="comment">/* work array for storing the block array S_jk, size m*m*cnp*cnp */</span>
<a name="l00574"></a>00574 <span class="keywordtype">double</span> *dp;   <span class="comment">/* work array for storing the parameter vector updates da_1, ..., da_m, db_1, ..., db_n, size m*cnp + n*pnp */</span>
<a name="l00575"></a>00575 <span class="keywordtype">double</span> *Wtda; <span class="comment">/* work array for storing \sum_j W_ij^T da_j, size pnp */</span>
<a name="l00576"></a>00576 <span class="keywordtype">double</span> *wght= <span class="comment">/* work array for storing the weights computed from the covariance inverses, max. size n*m*mnp*mnp */</span>
<a name="l00577"></a>00577             NULL;
<a name="l00578"></a>00578 
<a name="l00579"></a>00579 <span class="comment">/* Of the above arrays, jac, e, W, Yj, wght are sparse and</span>
<a name="l00580"></a>00580 <span class="comment"> * U, V, eab, E, S, dp are dense. Sparse arrays (except Yj) are indexed</span>
<a name="l00581"></a>00581 <span class="comment"> * through idxij (see below), that is with the same mechanism as the input </span>
<a name="l00582"></a>00582 <span class="comment"> * measurements vector x</span>
<a name="l00583"></a>00583 <span class="comment"> */</span>
<a name="l00584"></a>00584 
<a name="l00585"></a>00585 <span class="keywordtype">double</span> *pa, *pb, *ea, *eb, *dpa, *dpb; <span class="comment">/* pointers into p, jac, eab and dp respectively */</span>
<a name="l00586"></a>00586 
<a name="l00587"></a>00587 <span class="comment">/* submatrices sizes */</span>
<a name="l00588"></a>00588 <span class="keywordtype">int</span> Asz, Bsz, ABsz, Usz, Vsz,
<a name="l00589"></a>00589     Wsz, Ysz, esz, easz, ebsz,
<a name="l00590"></a>00590     YWtsz, Wtdasz, Sblsz, covsz;
<a name="l00591"></a>00591 
<a name="l00592"></a>00592 <span class="keywordtype">int</span> Sdim; <span class="comment">/* S matrix actual dimension */</span>
<a name="l00593"></a>00593 
<a name="l00594"></a>00594 <span class="keyword">register</span> <span class="keywordtype">double</span> *ptr1, *ptr2, *ptr3, *ptr4, sum;
<a name="l00595"></a>00595 <span class="keyword">struct </span>sba_crsm idxij; <span class="comment">/* sparse matrix containing the location of x_ij in x. This is also</span>
<a name="l00596"></a>00596 <span class="comment">                        * the location of A_ij, B_ij in jac, etc.</span>
<a name="l00597"></a>00597 <span class="comment">                        * This matrix can be thought as a map from a sparse set of pairs (i, j) to a continuous</span>
<a name="l00598"></a>00598 <span class="comment">                        * index k and it is used to efficiently lookup the memory locations where the non-zero</span>
<a name="l00599"></a>00599 <span class="comment">                        * blocks of a sparse matrix/vector are stored</span>
<a name="l00600"></a>00600 <span class="comment">                        */</span>
<a name="l00601"></a>00601 <span class="keywordtype">int</span> maxCvis, <span class="comment">/* max. of projections of a single point  across cameras, &lt;=m */</span>
<a name="l00602"></a>00602     maxPvis, <span class="comment">/* max. of projections in a single camera across points,  &lt;=n */</span>
<a name="l00603"></a>00603     maxCPvis, <span class="comment">/* max. of the above */</span>
<a name="l00604"></a>00604     *rcidxs,  <span class="comment">/* work array for the indexes corresponding to the nonzero elements of a single row or</span>
<a name="l00605"></a>00605 <span class="comment">                 column in a sparse matrix, size max(n, m) */</span>
<a name="l00606"></a>00606     *rcsubs;  <span class="comment">/* work array for the subscripts of nonzero elements in a single row or column of a</span>
<a name="l00607"></a>00607 <span class="comment">                 sparse matrix, size max(n, m) */</span>
<a name="l00608"></a>00608 
<a name="l00609"></a>00609 <span class="comment">/* The following variables are needed by the LM algorithm */</span>
<a name="l00610"></a>00610 <span class="keyword">register</span> <span class="keywordtype">int</span> itno;  <span class="comment">/* iteration counter */</span>
<a name="l00611"></a>00611 <span class="keywordtype">int</span> issolved;
<a name="l00612"></a>00612 <span class="comment">/* temporary work arrays that are dynamically allocated */</span>
<a name="l00613"></a>00613 <span class="keywordtype">double</span> *hx,         <span class="comment">/* \hat{x}_i, max. size m*n*mnp */</span>
<a name="l00614"></a>00614        *diagUV,     <span class="comment">/* diagonals of U_j, V_i, size m*cnp + n*pnp */</span>
<a name="l00615"></a>00615        *pdp;        <span class="comment">/* p + dp, size m*cnp + n*pnp */</span>
<a name="l00616"></a>00616 
<a name="l00617"></a>00617 <span class="keywordtype">double</span> *diagU, *diagV; <span class="comment">/* pointers into diagUV */</span>
<a name="l00618"></a>00618 
<a name="l00619"></a>00619 <span class="keyword">register</span> <span class="keywordtype">double</span> mu,  <span class="comment">/* damping constant */</span>
<a name="l00620"></a>00620                 tmp; <span class="comment">/* mainly used in matrix &amp; vector multiplications */</span>
<a name="l00621"></a>00621 <span class="keywordtype">double</span> p_eL2, eab_inf, pdp_eL2; <span class="comment">/* ||e(p)||_2, ||J^T e||_inf, ||e(p+dp)||_2 */</span>
<a name="l00622"></a>00622 <span class="keywordtype">double</span> p_L2, dp_L2=DBL_MAX, dF, dL;
<a name="l00623"></a>00623 <span class="keywordtype">double</span> tau=FABS(opts[0]), eps1=FABS(opts[1]), eps2=FABS(opts[2]), eps2_sq=opts[2]*opts[2],
<a name="l00624"></a>00624        eps3_sq=opts[3]*opts[3], eps4_sq=opts[4]*opts[4];
<a name="l00625"></a>00625 <span class="keywordtype">double</span> init_p_eL2;
<a name="l00626"></a>00626 <span class="keywordtype">int</span> nu=2, nu2, stop=0, nfev, njev=0, nlss=0;
<a name="l00627"></a>00627 <span class="keywordtype">int</span> nobs, nvars;
<a name="l00628"></a>00628 <span class="keyword">const</span> <span class="keywordtype">int</span> mmcon=m-mcon;
<a name="l00629"></a>00629 PLS linsolver=NULL;
<a name="l00630"></a>00630 int (*matinv)(<span class="keywordtype">double</span> *A, <span class="keywordtype">int</span> m)=NULL;
<a name="l00631"></a>00631 
<a name="l00632"></a>00632 <span class="keyword">struct </span>fdj_data_x_ fdj_data;
<a name="l00633"></a>00633 <span class="keywordtype">void</span> *jac_adata;
<a name="l00634"></a>00634 
<a name="l00635"></a>00635 <span class="comment">/* Initialization */</span>
<a name="l00636"></a>00636   mu=eab_inf=0.0; <span class="comment">/* -Wall */</span>
<a name="l00637"></a>00637 
<a name="l00638"></a>00638   <span class="comment">/* block sizes */</span>
<a name="l00639"></a>00639   Asz=mnp * cnp; Bsz=mnp * pnp; ABsz=Asz + Bsz;
<a name="l00640"></a>00640   Usz=cnp * cnp; Vsz=pnp * pnp;
<a name="l00641"></a>00641   Wsz=cnp * pnp; Ysz=cnp * pnp;
<a name="l00642"></a>00642   esz=mnp;
<a name="l00643"></a>00643   easz=cnp; ebsz=pnp;
<a name="l00644"></a>00644   YWtsz=cnp * cnp;
<a name="l00645"></a>00645   Wtdasz=pnp;
<a name="l00646"></a>00646   Sblsz=cnp * cnp;
<a name="l00647"></a>00647   Sdim=mmcon * cnp;
<a name="l00648"></a>00648   covsz=mnp * mnp;
<a name="l00649"></a>00649 
<a name="l00650"></a>00650   <span class="comment">/* count total number of visible image points */</span>
<a name="l00651"></a>00651   <span class="keywordflow">for</span>(i=nvis=0, jj=n*m; i&lt;jj; ++i)
<a name="l00652"></a>00652     nvis+=(vmask[i]!=0);
<a name="l00653"></a>00653 
<a name="l00654"></a>00654   nobs=nvis*mnp;
<a name="l00655"></a>00655   nvars=m*cnp + n*pnp;
<a name="l00656"></a>00656   <span class="keywordflow">if</span>(nobs&lt;nvars){
<a name="l00657"></a>00657     fprintf(stderr, <span class="stringliteral">&quot;SBA: sba_motstr_levmar_x() cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n&quot;</span>, nobs, nvars);
<a name="l00658"></a>00658     <span class="keywordflow">return</span> SBA_ERROR;
<a name="l00659"></a>00659   }
<a name="l00660"></a>00660 
<a name="l00661"></a>00661   <span class="comment">/* allocate &amp; fill up the idxij structure */</span>
<a name="l00662"></a>00662   sba_crsm_alloc(&amp;idxij, n, m, nvis);
<a name="l00663"></a>00663   <span class="keywordflow">for</span>(i=k=0; i&lt;n; ++i){
<a name="l00664"></a>00664     idxij.rowptr[i]=k;
<a name="l00665"></a>00665     ii=i*m;
<a name="l00666"></a>00666     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l00667"></a>00667       <span class="keywordflow">if</span>(vmask[ii+j]){
<a name="l00668"></a>00668         idxij.val[k]=k;
<a name="l00669"></a>00669         idxij.colidx[k++]=j;
<a name="l00670"></a>00670       }
<a name="l00671"></a>00671   }
<a name="l00672"></a>00672   idxij.rowptr[n]=nvis;
<a name="l00673"></a>00673 
<a name="l00674"></a>00674   <span class="comment">/* find the maximum number (for all cameras) of visible image projections coming from a single 3D point */</span>
<a name="l00675"></a>00675   <span class="keywordflow">for</span>(i=maxCvis=0; i&lt;n; ++i)
<a name="l00676"></a>00676     <span class="keywordflow">if</span>((k=idxij.rowptr[i+1]-idxij.rowptr[i])&gt;maxCvis) maxCvis=k;
<a name="l00677"></a>00677 
<a name="l00678"></a>00678   <span class="comment">/* find the maximum number (for all points) of visible image projections in any single camera */</span>
<a name="l00679"></a>00679   <span class="keywordflow">for</span>(j=maxPvis=0; j&lt;m; ++j){
<a name="l00680"></a>00680     <span class="keywordflow">for</span>(i=ii=0; i&lt;n; ++i)
<a name="l00681"></a>00681       <span class="keywordflow">if</span>(vmask[i*m+j]) ++ii;
<a name="l00682"></a>00682     <span class="keywordflow">if</span>(ii&gt;maxPvis) maxPvis=ii;
<a name="l00683"></a>00683   }
<a name="l00684"></a>00684   maxCPvis=(maxCvis&gt;=maxPvis)? maxCvis : maxPvis;
<a name="l00685"></a>00685 
<a name="l00686"></a>00686 <span class="preprocessor">#if 0</span>
<a name="l00687"></a>00687 <span class="preprocessor"></span>  <span class="comment">/* determine the density of blocks in matrix S */</span>
<a name="l00688"></a>00688   <span class="keywordflow">for</span>(j=mcon, ii=0; j&lt;m; ++j){
<a name="l00689"></a>00689     ++ii; <span class="comment">/* block Sjj is surely nonzero */</span>
<a name="l00690"></a>00690     <span class="keywordflow">for</span>(k=j+1; k&lt;m; ++k)
<a name="l00691"></a>00691       <span class="keywordflow">if</span>(sba_crsm_common_row(&amp;idxij, j, k)) ii+=2; <span class="comment">/* blocks Sjk &amp; Skj are nonzero */</span>
<a name="l00692"></a>00692   }
<a name="l00693"></a>00693   printf(<span class="stringliteral">&quot;\nS block density: %.5g\n&quot;</span>, ((<span class="keywordtype">double</span>)ii)/(mmcon*mmcon)); fflush(stdout);
<a name="l00694"></a>00694 <span class="preprocessor">#endif</span>
<a name="l00695"></a>00695 <span class="preprocessor"></span>
<a name="l00696"></a>00696   <span class="comment">/* allocate work arrays */</span>
<a name="l00697"></a>00697   <span class="comment">/* W is big enough to hold both jac &amp; W. Note also the extra Wsz, see the initialization of jac below for explanation */</span>
<a name="l00698"></a>00698   W=(<span class="keywordtype">double</span> *)emalloc((nvis*((Wsz&gt;=ABsz)? Wsz : ABsz) + Wsz)*<span class="keyword">sizeof</span>(double));
<a name="l00699"></a>00699   U=(<span class="keywordtype">double</span> *)emalloc(m*Usz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00700"></a>00700   V=(<span class="keywordtype">double</span> *)emalloc(n*Vsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00701"></a>00701   e=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00702"></a>00702   eab=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00703"></a>00703   E=(<span class="keywordtype">double</span> *)emalloc(m*cnp*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00704"></a>00704   Yj=(<span class="keywordtype">double</span> *)emalloc(maxPvis*Ysz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00705"></a>00705   YWt=(<span class="keywordtype">double</span> *)emalloc(YWtsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00706"></a>00706   S=(<span class="keywordtype">double</span> *)emalloc(m*m*Sblsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00707"></a>00707   dp=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00708"></a>00708   Wtda=(<span class="keywordtype">double</span> *)emalloc(pnp*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00709"></a>00709   rcidxs=(<span class="keywordtype">int</span> *)emalloc(maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l00710"></a>00710   rcsubs=(<span class="keywordtype">int</span> *)emalloc(maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l00711"></a>00711 <span class="preprocessor">#ifndef SBA_DESTROY_COVS</span>
<a name="l00712"></a>00712 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(covx!=NULL) wght=(<span class="keywordtype">double</span> *)emalloc(nvis*covsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00713"></a>00713 <span class="preprocessor">#else</span>
<a name="l00714"></a>00714 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(covx!=NULL) wght=covx;
<a name="l00715"></a>00715 <span class="preprocessor">#endif </span><span class="comment">/* SBA_DESTROY_COVS */</span>
<a name="l00716"></a>00716 
<a name="l00717"></a>00717 
<a name="l00718"></a>00718   hx=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00719"></a>00719   diagUV=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00720"></a>00720   pdp=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00721"></a>00721 
<a name="l00722"></a>00722   <span class="comment">/* to save resources, W and jac share the same memory: First, the jacobian</span>
<a name="l00723"></a>00723 <span class="comment">   * is computed in some working memory that is then overwritten during the</span>
<a name="l00724"></a>00724 <span class="comment">   * computation of W. To account for the case of W being larger than jac,</span>
<a name="l00725"></a>00725 <span class="comment">   * extra memory is reserved &quot;before&quot; jac.</span>
<a name="l00726"></a>00726 <span class="comment">   * Care must be taken, however, to ensure that storing a certain W_ij</span>
<a name="l00727"></a>00727 <span class="comment">   * does not overwrite the A_ij, B_ij used to compute it. To achieve</span>
<a name="l00728"></a>00728 <span class="comment">   * this is, note that if p1 and p2 respectively point to the first elements</span>
<a name="l00729"></a>00729 <span class="comment">   * of a certain W_ij and A_ij, B_ij pair, we should have p2-p1&gt;=Wsz.</span>
<a name="l00730"></a>00730 <span class="comment">   * There are two cases:</span>
<a name="l00731"></a>00731 <span class="comment">   * a) Wsz&gt;=ABsz: Then p1=W+k*Wsz and p2=jac+k*ABsz=W+Wsz+nvis*(Wsz-ABsz)+k*ABsz</span>
<a name="l00732"></a>00732 <span class="comment">   *    for some k (0&lt;=k&lt;nvis), thus p2-p1=(nvis-k)*(Wsz-ABsz)+Wsz. </span>
<a name="l00733"></a>00733 <span class="comment">   *    The right side of the last equation is obviously &gt; Wsz for all 0&lt;=k&lt;nvis</span>
<a name="l00734"></a>00734 <span class="comment">   *</span>
<a name="l00735"></a>00735 <span class="comment">   * b) Wsz&lt;ABsz: Then p1=W+k*Wsz and p2=jac+k*ABsz=W+Wsz+k*ABsz and</span>
<a name="l00736"></a>00736 <span class="comment">   *    p2-p1=Wsz+k*(ABsz-Wsz), which is again &gt; Wsz for all 0&lt;=k&lt;nvis</span>
<a name="l00737"></a>00737 <span class="comment">   *</span>
<a name="l00738"></a>00738 <span class="comment">   * In conclusion, if jac is initialized as below, the memory allocated to all</span>
<a name="l00739"></a>00739 <span class="comment">   * W_ij is guaranteed not to overlap with that allocated to their corresponding</span>
<a name="l00740"></a>00740 <span class="comment">   * A_ij, B_ij pairs</span>
<a name="l00741"></a>00741 <span class="comment">   */</span>
<a name="l00742"></a>00742   jac=W + Wsz + ((Wsz&gt;ABsz)? nvis*(Wsz-ABsz) : 0);
<a name="l00743"></a>00743 
<a name="l00744"></a>00744   <span class="comment">/* set up auxiliary pointers */</span>
<a name="l00745"></a>00745   pa=p; pb=p+m*cnp;
<a name="l00746"></a>00746   ea=eab; eb=eab+m*cnp;
<a name="l00747"></a>00747   dpa=dp; dpb=dp+m*cnp;
<a name="l00748"></a>00748 
<a name="l00749"></a>00749   diagU=diagUV; diagV=diagUV + m*cnp;
<a name="l00750"></a>00750 
<a name="l00751"></a>00751   <span class="comment">/* if no jacobian function is supplied, prepare to compute jacobian with finite difference */</span>
<a name="l00752"></a>00752   <span class="keywordflow">if</span>(!fjac){
<a name="l00753"></a>00753     fdj_data.func=func;
<a name="l00754"></a>00754     fdj_data.cnp=cnp;
<a name="l00755"></a>00755     fdj_data.pnp=pnp;
<a name="l00756"></a>00756     fdj_data.mnp=mnp;
<a name="l00757"></a>00757     fdj_data.hx=hx;
<a name="l00758"></a>00758     fdj_data.hxx=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00759"></a>00759     fdj_data.func_rcidxs=(<span class="keywordtype">int</span> *)emalloc(2*maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l00760"></a>00760     fdj_data.func_rcsubs=fdj_data.func_rcidxs+maxCPvis;
<a name="l00761"></a>00761     fdj_data.adata=adata;
<a name="l00762"></a>00762 
<a name="l00763"></a>00763     fjac=sba_fdjac_x;
<a name="l00764"></a>00764     jac_adata=(<span class="keywordtype">void</span> *)&amp;fdj_data;
<a name="l00765"></a>00765   }
<a name="l00766"></a>00766   <span class="keywordflow">else</span>{
<a name="l00767"></a>00767     fdj_data.hxx=NULL;
<a name="l00768"></a>00768     jac_adata=adata;
<a name="l00769"></a>00769   }
<a name="l00770"></a>00770 
<a name="l00771"></a>00771   <span class="keywordflow">if</span>(itmax==0){ <span class="comment">/* verify jacobian */</span>
<a name="l00772"></a>00772     sba_motstr_chkjac_x(func, fjac, p, &amp;idxij, rcidxs, rcsubs, ncon, mcon, cnp, pnp, mnp, adata, jac_adata);
<a name="l00773"></a>00773     retval=0;
<a name="l00774"></a>00774     <span class="keywordflow">goto</span> freemem_and_return;
<a name="l00775"></a>00775   }
<a name="l00776"></a>00776 
<a name="l00777"></a>00777   <span class="comment">/* covariances Sigma_x_ij are accommodated by computing the Cholesky decompositions of their</span>
<a name="l00778"></a>00778 <span class="comment">   * inverses and using the resulting matrices w_x_ij to weigh A_ij, B_ij, and e_ij as w_x_ij A_ij,</span>
<a name="l00779"></a>00779 <span class="comment">   * w_x_ij*B_ij and w_x_ij*e_ij. In this way, auxiliary variables as U_j=\sum_i A_ij^T A_ij</span>
<a name="l00780"></a>00780 <span class="comment">   * actually become \sum_i (w_x_ij A_ij)^T w_x_ij A_ij= \sum_i A_ij^T w_x_ij^T w_x_ij A_ij =</span>
<a name="l00781"></a>00781 <span class="comment">   * A_ij^T Sigma_x_ij^-1 A_ij</span>
<a name="l00782"></a>00782 <span class="comment">   *</span>
<a name="l00783"></a>00783 <span class="comment">   * ea_j, V_i, eb_i, W_ij are weighted in a similar manner</span>
<a name="l00784"></a>00784 <span class="comment">   */</span>
<a name="l00785"></a>00785   <span class="keywordflow">if</span>(covx!=NULL){
<a name="l00786"></a>00786     <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l00787"></a>00787       nnz=sba_crsm_row_elmidxs(&amp;idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero x_ij, j=0...m-1 */</span>
<a name="l00788"></a>00788       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l00789"></a>00789         <span class="comment">/* set ptr1, ptr2 to point to cov_x_ij, w_x_ij resp. */</span>
<a name="l00790"></a>00790         ptr1=covx + (k=idxij.val[rcidxs[j]]*covsz);
<a name="l00791"></a>00791         ptr2=wght + k;
<a name="l00792"></a>00792         <span class="keywordflow">if</span>(!sba_mat_cholinv(ptr1, ptr2, mnp)){ <span class="comment">/* compute w_x_ij s.t. w_x_ij^T w_x_ij = cov_x_ij^-1 */</span>
<a name="l00793"></a>00793                             fprintf(stderr, <span class="stringliteral">&quot;SBA: invalid covariance matrix for x_ij (i=%d, j=%d) in sba_motstr_levmar_x()\n&quot;</span>, i, rcsubs[j]);
<a name="l00794"></a>00794           retval=SBA_ERROR;
<a name="l00795"></a>00795           <span class="keywordflow">goto</span> freemem_and_return;
<a name="l00796"></a>00796         }
<a name="l00797"></a>00797       }
<a name="l00798"></a>00798     }
<a name="l00799"></a>00799     sba_mat_cholinv(NULL, NULL, 0); <span class="comment">/* cleanup */</span>
<a name="l00800"></a>00800   }
<a name="l00801"></a>00801 
<a name="l00802"></a>00802   <span class="comment">/* compute the error vectors e_ij in hx */</span>
<a name="l00803"></a>00803   (*func)(p, &amp;idxij, rcidxs, rcsubs, hx, adata); nfev=1;
<a name="l00804"></a>00804   <span class="comment">/* ### compute e=x - f(p) [e=w*(x - f(p)] and its L2 norm */</span>
<a name="l00805"></a>00805   <span class="keywordflow">if</span>(covx==NULL)
<a name="l00806"></a>00806     p_eL2=nrmL2xmy(e, x, hx, nobs); <span class="comment">/* e=x-hx, p_eL2=||e|| */</span>
<a name="l00807"></a>00807   <span class="keywordflow">else</span>
<a name="l00808"></a>00808     p_eL2=nrmCxmy(e, x, hx, wght, mnp, nvis); <span class="comment">/* e=wght*(x-hx), p_eL2=||e||=||x-hx||_Sigma^-1 */</span>
<a name="l00809"></a>00809   <span class="keywordflow">if</span>(verbose) printf(<span class="stringliteral">&quot;initial motstr-SBA error %g [%g]\n&quot;</span>, p_eL2, p_eL2/nvis);
<a name="l00810"></a>00810   init_p_eL2=p_eL2;
<a name="l00811"></a>00811   <span class="keywordflow">if</span>(!SBA_FINITE(p_eL2)) stop=7;
<a name="l00812"></a>00812 
<a name="l00813"></a>00813   <span class="keywordflow">for</span>(itno=0; itno&lt;itmax &amp;&amp; !stop; ++itno){
<a name="l00814"></a>00814     <span class="comment">/* Note that p, e and ||e||_2 have been updated at the previous iteration */</span>
<a name="l00815"></a>00815 
<a name="l00816"></a>00816     <span class="comment">/* compute derivative submatrices A_ij, B_ij */</span>
<a name="l00817"></a>00817     (*fjac)(p, &amp;idxij, rcidxs, rcsubs, jac, jac_adata); ++njev;
<a name="l00818"></a>00818 
<a name="l00819"></a>00819     <span class="keywordflow">if</span>(covx!=NULL){
<a name="l00820"></a>00820       <span class="comment">/* compute w_x_ij A_ij and w_x_ij B_ij.</span>
<a name="l00821"></a>00821 <span class="comment">       * Since w_x_ij is upper triangular, the products can be safely saved</span>
<a name="l00822"></a>00822 <span class="comment">       * directly in A_ij, B_ij, without the need for intermediate storage</span>
<a name="l00823"></a>00823 <span class="comment">       */</span>
<a name="l00824"></a>00824       <span class="keywordflow">for</span>(i=0; i&lt;nvis; ++i){
<a name="l00825"></a>00825         <span class="comment">/* set ptr1, ptr2, ptr3 to point to w_x_ij, A_ij, B_ij, resp. */</span>
<a name="l00826"></a>00826         ptr1=wght + i*covsz;
<a name="l00827"></a>00827         ptr2=jac  + i*ABsz;
<a name="l00828"></a>00828         ptr3=ptr2 + Asz; <span class="comment">// ptr3=jac  + i*ABsz + Asz;</span>
<a name="l00829"></a>00829 
<a name="l00830"></a>00830         <span class="comment">/* w_x_ij is mnp x mnp, A_ij is mnp x cnp, B_ij is mnp x pnp</span>
<a name="l00831"></a>00831 <span class="comment">         * NOTE: Jamming the outter (i.e., ii) loops did not run faster!</span>
<a name="l00832"></a>00832 <span class="comment">         */</span>
<a name="l00833"></a>00833         <span class="comment">/* A_ij */</span>
<a name="l00834"></a>00834         <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00835"></a>00835           <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj){
<a name="l00836"></a>00836             <span class="keywordflow">for</span>(k=ii, sum=0.0; k&lt;mnp; ++k) <span class="comment">// k&gt;=ii since w_x_ij is upper triangular</span>
<a name="l00837"></a>00837               sum+=ptr1[ii*mnp+k]*ptr2[k*cnp+jj];
<a name="l00838"></a>00838             ptr2[ii*cnp+jj]=sum;
<a name="l00839"></a>00839           }
<a name="l00840"></a>00840 
<a name="l00841"></a>00841         <span class="comment">/* B_ij */</span>
<a name="l00842"></a>00842         <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00843"></a>00843           <span class="keywordflow">for</span>(jj=0; jj&lt;pnp; ++jj){
<a name="l00844"></a>00844             <span class="keywordflow">for</span>(k=ii, sum=0.0; k&lt;mnp; ++k) <span class="comment">// k&gt;=ii since w_x_ij is upper triangular</span>
<a name="l00845"></a>00845               sum+=ptr1[ii*mnp+k]*ptr3[k*pnp+jj];
<a name="l00846"></a>00846             ptr3[ii*pnp+jj]=sum;
<a name="l00847"></a>00847           }
<a name="l00848"></a>00848       }
<a name="l00849"></a>00849     }
<a name="l00850"></a>00850 
<a name="l00851"></a>00851     <span class="comment">/* compute U_j = \sum_i A_ij^T A_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l00852"></a>00852     <span class="comment">/* U_j is symmetric, therefore its computation can be sped up by</span>
<a name="l00853"></a>00853 <span class="comment">     * computing only the upper part and then reusing it for the lower one.</span>
<a name="l00854"></a>00854 <span class="comment">     * Recall that A_ij is mnp x cnp</span>
<a name="l00855"></a>00855 <span class="comment">     */</span>
<a name="l00856"></a>00856     <span class="comment">/* Also compute ea_j = \sum_i A_ij^T e_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l00857"></a>00857     <span class="comment">/* Recall that e_ij is mnp x 1</span>
<a name="l00858"></a>00858 <span class="comment">     */</span>
<a name="l00859"></a>00859     _dblzero(U, m*Usz); <span class="comment">/* clear all U_j */</span>
<a name="l00860"></a>00860     _dblzero(ea, m*easz); <span class="comment">/* clear all ea_j */</span>
<a name="l00861"></a>00861     <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l00862"></a>00862       ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l00863"></a>00863       ptr2=ea + j*easz; <span class="comment">// set ptr2 to point to ea_j</span>
<a name="l00864"></a>00864 
<a name="l00865"></a>00865       nnz=sba_crsm_col_elmidxs(&amp;idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero A_ij, i=0...n-1 */</span>
<a name="l00866"></a>00866       <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00867"></a>00867         <span class="comment">/* set ptr3 to point to A_ij, actual row number in rcsubs[i] */</span>
<a name="l00868"></a>00868         ptr3=jac + idxij.val[rcidxs[i]]*ABsz;
<a name="l00869"></a>00869 
<a name="l00870"></a>00870         <span class="comment">/* compute the UPPER TRIANGULAR PART of A_ij^T A_ij and add it to U_j */</span>
<a name="l00871"></a>00871         <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii){
<a name="l00872"></a>00872           <span class="keywordflow">for</span>(jj=ii; jj&lt;cnp; ++jj){
<a name="l00873"></a>00873             <span class="keywordflow">for</span>(k=0, sum=0.0; k&lt;mnp; ++k)
<a name="l00874"></a>00874               sum+=ptr3[k*cnp+ii]*ptr3[k*cnp+jj];
<a name="l00875"></a>00875             ptr1[ii*cnp+jj]+=sum;
<a name="l00876"></a>00876           }
<a name="l00877"></a>00877 
<a name="l00878"></a>00878           <span class="comment">/* copy the LOWER TRIANGULAR PART of U_j from the upper one */</span>
<a name="l00879"></a>00879           <span class="keywordflow">for</span>(jj=0; jj&lt;ii; ++jj)
<a name="l00880"></a>00880             ptr1[ii*cnp+jj]=ptr1[jj*cnp+ii];
<a name="l00881"></a>00881         }
<a name="l00882"></a>00882 
<a name="l00883"></a>00883         ptr4=e + idxij.val[rcidxs[i]]*esz; <span class="comment">/* set ptr4 to point to e_ij */</span>
<a name="l00884"></a>00884         <span class="comment">/* compute A_ij^T e_ij and add it to ea_j */</span>
<a name="l00885"></a>00885         <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii){
<a name="l00886"></a>00886           <span class="keywordflow">for</span>(jj=0, sum=0.0; jj&lt;mnp; ++jj)
<a name="l00887"></a>00887             sum+=ptr3[jj*cnp+ii]*ptr4[jj];
<a name="l00888"></a>00888           ptr2[ii]+=sum;
<a name="l00889"></a>00889         }
<a name="l00890"></a>00890       }
<a name="l00891"></a>00891     }
<a name="l00892"></a>00892 
<a name="l00893"></a>00893     <span class="comment">/* compute V_i = \sum_j B_ij^T B_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l00894"></a>00894     <span class="comment">/* V_i is symmetric, therefore its computation can be sped up by</span>
<a name="l00895"></a>00895 <span class="comment">     * computing only the upper part and then reusing it for the lower one.</span>
<a name="l00896"></a>00896 <span class="comment">     * Recall that B_ij is mnp x pnp</span>
<a name="l00897"></a>00897 <span class="comment">     */</span>
<a name="l00898"></a>00898     <span class="comment">/* Also compute eb_i = \sum_j B_ij^T e_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l00899"></a>00899     <span class="comment">/* Recall that e_ij is mnp x 1</span>
<a name="l00900"></a>00900 <span class="comment">     */</span>
<a name="l00901"></a>00901           _dblzero(V, n*Vsz); <span class="comment">/* clear all V_i */</span>
<a name="l00902"></a>00902           _dblzero(eb, n*ebsz); <span class="comment">/* clear all eb_i */</span>
<a name="l00903"></a>00903     <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l00904"></a>00904       ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l00905"></a>00905       ptr2=eb + i*ebsz; <span class="comment">// set ptr2 to point to eb_i</span>
<a name="l00906"></a>00906 
<a name="l00907"></a>00907       nnz=sba_crsm_row_elmidxs(&amp;idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero B_ij, j=0...m-1 */</span>
<a name="l00908"></a>00908       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l00909"></a>00909         <span class="comment">/* set ptr3 to point to B_ij, actual column number in rcsubs[j] */</span>
<a name="l00910"></a>00910         ptr3=jac + idxij.val[rcidxs[j]]*ABsz + Asz;
<a name="l00911"></a>00911       
<a name="l00912"></a>00912         <span class="comment">/* compute the UPPER TRIANGULAR PART of B_ij^T B_ij and add it to V_i */</span>
<a name="l00913"></a>00913         <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii){
<a name="l00914"></a>00914           <span class="keywordflow">for</span>(jj=ii; jj&lt;pnp; ++jj){
<a name="l00915"></a>00915             <span class="keywordflow">for</span>(k=0, sum=0.0; k&lt;mnp; ++k)
<a name="l00916"></a>00916               sum+=ptr3[k*pnp+ii]*ptr3[k*pnp+jj];
<a name="l00917"></a>00917             ptr1[ii*pnp+jj]+=sum;
<a name="l00918"></a>00918           }
<a name="l00919"></a>00919         }
<a name="l00920"></a>00920 
<a name="l00921"></a>00921         ptr4=e + idxij.val[rcidxs[j]]*esz; <span class="comment">/* set ptr4 to point to e_ij */</span>
<a name="l00922"></a>00922         <span class="comment">/* compute B_ij^T e_ij and add it to eb_i */</span>
<a name="l00923"></a>00923         <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii){
<a name="l00924"></a>00924           <span class="keywordflow">for</span>(jj=0, sum=0.0; jj&lt;mnp; ++jj)
<a name="l00925"></a>00925             sum+=ptr3[jj*pnp+ii]*ptr4[jj];
<a name="l00926"></a>00926           ptr2[ii]+=sum;
<a name="l00927"></a>00927         }
<a name="l00928"></a>00928       }
<a name="l00929"></a>00929     }
<a name="l00930"></a>00930 
<a name="l00931"></a>00931     <span class="comment">/* compute W_ij =  A_ij^T B_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l00932"></a>00932     <span class="comment">/* Recall that A_ij is mnp x cnp and B_ij is mnp x pnp</span>
<a name="l00933"></a>00933 <span class="comment">     */</span>
<a name="l00934"></a>00934     <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l00935"></a>00935       nnz=sba_crsm_row_elmidxs(&amp;idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero W_ij, j=0...m-1 */</span>
<a name="l00936"></a>00936       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l00937"></a>00937         <span class="comment">/* set ptr1 to point to W_ij, actual column number in rcsubs[j] */</span>
<a name="l00938"></a>00938         ptr1=W + idxij.val[rcidxs[j]]*Wsz;
<a name="l00939"></a>00939 
<a name="l00940"></a>00940         <span class="keywordflow">if</span>(rcsubs[j]&lt;mcon){ <span class="comment">/* A_ij is zero */</span>
<a name="l00941"></a>00941           <span class="comment">//_dblzero(ptr1, Wsz); /* clear W_ij */</span>
<a name="l00942"></a>00942           <span class="keywordflow">continue</span>;
<a name="l00943"></a>00943         }
<a name="l00944"></a>00944 
<a name="l00945"></a>00945         <span class="comment">/* set ptr2 &amp; ptr3 to point to A_ij &amp; B_ij resp. */</span>
<a name="l00946"></a>00946         ptr2=jac  + idxij.val[rcidxs[j]]*ABsz;
<a name="l00947"></a>00947         ptr3=ptr2 + Asz;
<a name="l00948"></a>00948         <span class="comment">/* compute A_ij^T B_ij and store it in W_ij</span>
<a name="l00949"></a>00949 <span class="comment">         * Recall that storage for A_ij, B_ij does not overlap with that for W_ij,</span>
<a name="l00950"></a>00950 <span class="comment">         * see the comments related to the initialization of jac above</span>
<a name="l00951"></a>00951 <span class="comment">         */</span>
<a name="l00952"></a>00952         <span class="comment">/* assert(ptr2-ptr1&gt;=Wsz); */</span>
<a name="l00953"></a>00953         <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii)
<a name="l00954"></a>00954           <span class="keywordflow">for</span>(jj=0; jj&lt;pnp; ++jj){
<a name="l00955"></a>00955             <span class="keywordflow">for</span>(k=0, sum=0.0; k&lt;mnp; ++k)
<a name="l00956"></a>00956               sum+=ptr2[k*cnp+ii]*ptr3[k*pnp+jj];
<a name="l00957"></a>00957             ptr1[ii*pnp+jj]=sum;
<a name="l00958"></a>00958           }
<a name="l00959"></a>00959       }
<a name="l00960"></a>00960     }
<a name="l00961"></a>00961 
<a name="l00962"></a>00962     <span class="comment">/* Compute ||J^T e||_inf and ||p||^2 */</span>
<a name="l00963"></a>00963     <span class="keywordflow">for</span>(i=0, p_L2=eab_inf=0.0; i&lt;nvars; ++i){
<a name="l00964"></a>00964       <span class="keywordflow">if</span>(eab_inf &lt; (tmp=FABS(eab[i]))) eab_inf=tmp;
<a name="l00965"></a>00965       p_L2+=p[i]*p[i];
<a name="l00966"></a>00966     }
<a name="l00967"></a>00967     <span class="comment">//p_L2=sqrt(p_L2);</span>
<a name="l00968"></a>00968 
<a name="l00969"></a>00969     <span class="comment">/* save diagonal entries so that augmentation can be later canceled.</span>
<a name="l00970"></a>00970 <span class="comment">     * Diagonal entries are in U_j and V_i</span>
<a name="l00971"></a>00971 <span class="comment">     */</span>
<a name="l00972"></a>00972     <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l00973"></a>00973       ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l00974"></a>00974       ptr2=diagU + j*cnp; <span class="comment">// set ptr2 to point to diagU_j</span>
<a name="l00975"></a>00975       <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l00976"></a>00976         ptr2[i]=ptr1[i*cnp+i];
<a name="l00977"></a>00977     }
<a name="l00978"></a>00978     <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l00979"></a>00979       ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l00980"></a>00980       ptr2=diagV + i*pnp; <span class="comment">// set ptr2 to point to diagV_i</span>
<a name="l00981"></a>00981       <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l00982"></a>00982         ptr2[j]=ptr1[j*pnp+j];
<a name="l00983"></a>00983     }
<a name="l00984"></a>00984 
<a name="l00985"></a>00985 <span class="comment">/*</span>
<a name="l00986"></a>00986 <span class="comment">if(!(itno%100)){</span>
<a name="l00987"></a>00987 <span class="comment">  printf(&quot;Current estimate: &quot;);</span>
<a name="l00988"></a>00988 <span class="comment">  for(i=0; i&lt;nvars; ++i)</span>
<a name="l00989"></a>00989 <span class="comment">    printf(&quot;%.9g &quot;, p[i]);</span>
<a name="l00990"></a>00990 <span class="comment">  printf(&quot;-- errors %.9g %0.9g\n&quot;, eab_inf, p_eL2);</span>
<a name="l00991"></a>00991 <span class="comment">}</span>
<a name="l00992"></a>00992 <span class="comment">*/</span>
<a name="l00993"></a>00993 
<a name="l00994"></a>00994     <span class="comment">/* check for convergence */</span>
<a name="l00995"></a>00995     <span class="keywordflow">if</span>((eab_inf &lt;= eps1)){
<a name="l00996"></a>00996       dp_L2=0.0; <span class="comment">/* no increment for p in this case */</span>
<a name="l00997"></a>00997       stop=1;
<a name="l00998"></a>00998       <span class="keywordflow">break</span>;
<a name="l00999"></a>00999     }
<a name="l01000"></a>01000 
<a name="l01001"></a>01001    <span class="comment">/* compute initial damping factor */</span>
<a name="l01002"></a>01002     <span class="keywordflow">if</span>(itno==0){
<a name="l01003"></a>01003       <span class="comment">/* find max diagonal element */</span>
<a name="l01004"></a>01004       <span class="keywordflow">for</span>(i=mcon*cnp, tmp=DBL_MIN; i&lt;m*cnp; ++i)
<a name="l01005"></a>01005         <span class="keywordflow">if</span>(diagUV[i]&gt;tmp) tmp=diagUV[i];
<a name="l01006"></a>01006       <span class="keywordflow">for</span>(i=m*cnp + ncon*pnp; i&lt;nvars; ++i) <span class="comment">/* tmp is not re-initialized! */</span>
<a name="l01007"></a>01007         <span class="keywordflow">if</span>(diagUV[i]&gt;tmp) tmp=diagUV[i];
<a name="l01008"></a>01008       mu=tau*tmp;
<a name="l01009"></a>01009     }
<a name="l01010"></a>01010   <span class="comment">/* ...hacer algo... */</span>
<a name="l01011"></a>01011 <span class="comment">//clock_t t_fin = clock();</span>
<a name="l01012"></a>01012 
<a name="l01013"></a>01013 <span class="comment">//double time_initialization = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;</span>
<a name="l01014"></a>01014 
<a name="l01015"></a>01015     <span class="comment">/* determine increment using adaptive damping */</span>
<a name="l01016"></a>01016 <span class="comment">// Nota: Aqui se resuelve el sistema reducido de segundo nivel.</span>
<a name="l01017"></a>01017     <span class="keywordflow">while</span>(1){
<a name="l01018"></a>01018       <span class="comment">/* augment U, V */</span>
<a name="l01019"></a>01019       <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01020"></a>01020         ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01021"></a>01021         <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01022"></a>01022           ptr1[i*cnp+i]+=mu;
<a name="l01023"></a>01023       }
<a name="l01024"></a>01024       <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l01025"></a>01025         ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l01026"></a>01026         <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l01027"></a>01027           ptr1[j*pnp+j]+=mu;
<a name="l01028"></a>01028 
<a name="l01029"></a>01029                     <span class="comment">/* compute V*_i^-1.</span>
<a name="l01030"></a>01030 <span class="comment">         * Recall that only the upper triangle of the symmetric pnp x pnp matrix V*_i</span>
<a name="l01031"></a>01031 <span class="comment">         * is stored in ptr1; its (also symmetric) inverse is saved in the lower triangle of ptr1</span>
<a name="l01032"></a>01032 <span class="comment">         */</span>
<a name="l01033"></a>01033         <span class="comment">/* inverting V*_i with LDLT seems to result in faster overall execution compared to when using LU or Cholesky */</span>
<a name="l01034"></a>01034         <span class="comment">//j=sba_symat_invert_LU(ptr1, pnp); matinv=sba_symat_invert_LU;</span>
<a name="l01035"></a>01035         <span class="comment">//j=sba_symat_invert_Chol(ptr1, pnp); matinv=sba_symat_invert_Chol;</span>
<a name="l01036"></a>01036         j=sba_symat_invert_BK(ptr1, pnp); matinv=sba_symat_invert_BK;
<a name="l01037"></a>01037                     <span class="keywordflow">if</span>(!j){
<a name="l01038"></a>01038                             fprintf(stderr, <span class="stringliteral">&quot;SBA: singular matrix V*_i (i=%d) in sba_motstr_levmar_x(), increasing damping\n&quot;</span>, i);
<a name="l01039"></a>01039           <span class="keywordflow">goto</span> moredamping; <span class="comment">// increasing damping will eventually make V*_i diagonally dominant, thus nonsingular</span>
<a name="l01040"></a>01040           <span class="comment">//retval=SBA_ERROR;</span>
<a name="l01041"></a>01041           <span class="comment">//goto freemem_and_return;</span>
<a name="l01042"></a>01042                     }
<a name="l01043"></a>01043       }
<a name="l01044"></a>01044 
<a name="l01045"></a>01045       _dblzero(E, m*easz); <span class="comment">/* clear all e_j */</span>
<a name="l01046"></a>01046       <span class="comment">/* compute the mmcon x mmcon block matrix S and e_j */</span>
<a name="l01047"></a>01047 
<a name="l01048"></a>01048       <span class="comment">/* Note that S is symmetric, therefore its computation can be</span>
<a name="l01049"></a>01049 <span class="comment">       * sped up by computing only the upper part and then reusing</span>
<a name="l01050"></a>01050 <span class="comment">       * it for the lower one.</span>
<a name="l01051"></a>01051 <span class="comment">                   */</span>
<a name="l01052"></a>01052       <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01053"></a>01053         <span class="keywordtype">int</span> mmconxUsz=mmcon*Usz;
<a name="l01054"></a>01054 
<a name="l01055"></a>01055                     nnz=sba_crsm_col_elmidxs(&amp;idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero Y_ij, i=0...n-1 */</span>
<a name="l01056"></a>01056 
<a name="l01057"></a>01057         <span class="comment">/* get rid of all Y_ij with i&lt;ncon that are treated as zeros.</span>
<a name="l01058"></a>01058 <span class="comment">         * In this way, all rcsubs[i] below are guaranteed to be &gt;= ncon</span>
<a name="l01059"></a>01059 <span class="comment">         */</span>
<a name="l01060"></a>01060         <span class="keywordflow">if</span>(ncon){
<a name="l01061"></a>01061           <span class="keywordflow">for</span>(i=ii=0; i&lt;nnz; ++i){
<a name="l01062"></a>01062             <span class="keywordflow">if</span>(rcsubs[i]&gt;=ncon){
<a name="l01063"></a>01063               rcidxs[ii]=rcidxs[i];
<a name="l01064"></a>01064               rcsubs[ii++]=rcsubs[i];
<a name="l01065"></a>01065             }
<a name="l01066"></a>01066           }
<a name="l01067"></a>01067           nnz=ii;
<a name="l01068"></a>01068         }
<a name="l01069"></a>01069 
<a name="l01070"></a>01070         <span class="comment">/* compute all Y_ij = W_ij (V*_i)^-1 for a *fixed* j.</span>
<a name="l01071"></a>01071 <span class="comment">         * To save memory, the block matrix consisting of the Y_ij</span>
<a name="l01072"></a>01072 <span class="comment">         * is not stored. Instead, only a block column of this matrix</span>
<a name="l01073"></a>01073 <span class="comment">         * is computed &amp; used at each time: For each j, all nonzero</span>
<a name="l01074"></a>01074 <span class="comment">         * Y_ij are computed in Yj and then used in the calculations</span>
<a name="l01075"></a>01075 <span class="comment">         * involving S_jk and e_j.</span>
<a name="l01076"></a>01076 <span class="comment">         * Recall that W_ij is cnp x pnp and (V*_i) is pnp x pnp</span>
<a name="l01077"></a>01077 <span class="comment">         */</span>
<a name="l01078"></a>01078         <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l01079"></a>01079           <span class="comment">/* set ptr3 to point to (V*_i)^-1, actual row number in rcsubs[i] */</span>
<a name="l01080"></a>01080           ptr3=V + rcsubs[i]*Vsz;
<a name="l01081"></a>01081 
<a name="l01082"></a>01082           <span class="comment">/* set ptr1 to point to Y_ij, actual row number in rcsubs[i] */</span>
<a name="l01083"></a>01083           ptr1=Yj + i*Ysz;
<a name="l01084"></a>01084           <span class="comment">/* set ptr2 to point to W_ij resp. */</span>
<a name="l01085"></a>01085           ptr2=W + idxij.val[rcidxs[i]]*Wsz;
<a name="l01086"></a>01086           <span class="comment">/* compute W_ij (V*_i)^-1 and store it in Y_ij.</span>
<a name="l01087"></a>01087 <span class="comment">           * Recall that only the lower triangle of (V*_i)^-1 is stored</span>
<a name="l01088"></a>01088 <span class="comment">           */</span>
<a name="l01089"></a>01089           <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii){
<a name="l01090"></a>01090             ptr4=ptr2+ii*pnp;
<a name="l01091"></a>01091             <span class="keywordflow">for</span>(jj=0; jj&lt;pnp; ++jj){
<a name="l01092"></a>01092               <span class="keywordflow">for</span>(k=0, sum=0.0; k&lt;=jj; ++k)
<a name="l01093"></a>01093                 sum+=ptr4[k]*ptr3[jj*pnp+k]; <span class="comment">//ptr2[ii*pnp+k]*ptr3[jj*pnp+k];</span>
<a name="l01094"></a>01094               <span class="keywordflow">for</span>( ; k&lt;pnp; ++k)
<a name="l01095"></a>01095                 sum+=ptr4[k]*ptr3[k*pnp+jj]; <span class="comment">//ptr2[ii*pnp+k]*ptr3[k*pnp+jj];</span>
<a name="l01096"></a>01096               ptr1[ii*pnp+jj]=sum;
<a name="l01097"></a>01097             }
<a name="l01098"></a>01098           }
<a name="l01099"></a>01099         }
<a name="l01100"></a>01100 
<a name="l01101"></a>01101 <span class="comment">// Nota: computa la matriz S reducida de sistema. Primero se calcula la parte superior triangular, y luego se copia.</span>
<a name="l01102"></a>01102         <span class="comment">/* compute the UPPER TRIANGULAR PART of S */</span>
<a name="l01103"></a>01103         <span class="keywordflow">for</span>(k=j; k&lt;m; ++k){ <span class="comment">// j&gt;=mcon</span>
<a name="l01104"></a>01104           <span class="comment">/* compute \sum_i Y_ij W_ik^T in YWt. Note that for an off-diagonal block defined by j, k</span>
<a name="l01105"></a>01105 <span class="comment">           * YWt (and thus S_jk) is nonzero only if there exists a point that is visible in both the</span>
<a name="l01106"></a>01106 <span class="comment">           * j-th and k-th images</span>
<a name="l01107"></a>01107 <span class="comment">           */</span>
<a name="l01108"></a>01108           
<a name="l01109"></a>01109           <span class="comment">/* Recall that Y_ij is cnp x pnp and W_ik is cnp x pnp */</span> 
<a name="l01110"></a>01110           _dblzero(YWt, YWtsz); <span class="comment">/* clear YWt */</span>
<a name="l01111"></a>01111 
<a name="l01112"></a>01112           <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l01113"></a>01113             <span class="keyword">register</span> <span class="keywordtype">double</span> *pYWt;
<a name="l01114"></a>01114 
<a name="l01115"></a>01115             <span class="comment">/* find the min and max column indices of the elements in row i (actually rcsubs[i])</span>
<a name="l01116"></a>01116 <span class="comment">             * and make sure that k falls within them. This test handles W_ik&#39;s which are</span>
<a name="l01117"></a>01117 <span class="comment">             * certain to be zero without bothering to call sba_crsm_elmidx()</span>
<a name="l01118"></a>01118 <span class="comment">             */</span>
<a name="l01119"></a>01119             ii=idxij.colidx[idxij.rowptr[rcsubs[i]]];
<a name="l01120"></a>01120             jj=idxij.colidx[idxij.rowptr[rcsubs[i]+1]-1];
<a name="l01121"></a>01121             <span class="keywordflow">if</span>(k&lt;ii || k&gt;jj) <span class="keywordflow">continue</span>; <span class="comment">/* W_ik == 0 */</span>
<a name="l01122"></a>01122 
<a name="l01123"></a>01123             <span class="comment">/* set ptr2 to point to W_ik */</span>
<a name="l01124"></a>01124             l=sba_crsm_elmidxp(&amp;idxij, rcsubs[i], k, j, rcidxs[i]);
<a name="l01125"></a>01125             <span class="comment">//l=sba_crsm_elmidx(&amp;idxij, rcsubs[i], k);</span>
<a name="l01126"></a>01126             <span class="keywordflow">if</span>(l==-1) <span class="keywordflow">continue</span>; <span class="comment">/* W_ik == 0 */</span>
<a name="l01127"></a>01127 
<a name="l01128"></a>01128             ptr2=W + idxij.val[l]*Wsz;
<a name="l01129"></a>01129             <span class="comment">/* set ptr1 to point to Y_ij, actual row number in rcsubs[i] */</span>
<a name="l01130"></a>01130             ptr1=Yj + i*Ysz;
<a name="l01131"></a>01131             <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii){
<a name="l01132"></a>01132               ptr3=ptr1+ii*pnp;
<a name="l01133"></a>01133               pYWt=YWt+ii*cnp;
<a name="l01134"></a>01134 
<a name="l01135"></a>01135               <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj){
<a name="l01136"></a>01136                 ptr4=ptr2+jj*pnp;
<a name="l01137"></a>01137                 <span class="keywordflow">for</span>(l=0, sum=0.0; l&lt;pnp; ++l)
<a name="l01138"></a>01138                   sum+=ptr3[l]*ptr4[l]; <span class="comment">//ptr1[ii*pnp+l]*ptr2[jj*pnp+l];</span>
<a name="l01139"></a>01139                 pYWt[jj]+=sum; <span class="comment">//YWt[ii*cnp+jj]+=sum;</span>
<a name="l01140"></a>01140               }
<a name="l01141"></a>01141             }
<a name="l01142"></a>01142           }
<a name="l01143"></a>01143                   
<a name="l01144"></a>01144                       <span class="comment">/* since the linear system involving S is solved with lapack,</span>
<a name="l01145"></a>01145 <span class="comment">                       * it is preferable to store S in column major (i.e. fortran)</span>
<a name="l01146"></a>01146 <span class="comment">                       * order, so as to avoid unecessary transposing/copying.</span>
<a name="l01147"></a>01147 <span class="comment">           */</span>
<a name="l01148"></a>01148 
<a name="l01149"></a>01149 <span class="preprocessor">#if MAT_STORAGE==COLUMN_MAJOR</span>
<a name="l01150"></a>01150 <span class="preprocessor"></span>          ptr2=S + (k-mcon)*mmconxUsz + (j-mcon)*cnp; <span class="comment">// set ptr2 to point to the beginning of block j,k in S</span>
<a name="l01151"></a>01151 <span class="preprocessor">#else</span>
<a name="l01152"></a>01152 <span class="preprocessor"></span>          ptr2=S + (j-mcon)*mmconxUsz + (k-mcon)*cnp; <span class="comment">// set ptr2 to point to the beginning of block j,k in S</span>
<a name="l01153"></a>01153 <span class="preprocessor">#endif</span>
<a name="l01154"></a>01154 <span class="preprocessor"></span>                  
<a name="l01155"></a>01155           <span class="keywordflow">if</span>(j!=k){ <span class="comment">/* Kronecker */</span>
<a name="l01156"></a>01156             <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii, ptr2+=Sdim)
<a name="l01157"></a>01157               <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj)
<a name="l01158"></a>01158                 ptr2[jj]=
<a name="l01159"></a>01159 #<span class="keywordflow">if</span> MAT_STORAGE==COLUMN_MAJOR
<a name="l01160"></a>01160                                                 -YWt[jj*cnp+ii];
<a name="l01161"></a>01161 <span class="preprocessor">#else</span>
<a name="l01162"></a>01162 <span class="preprocessor"></span>                                                -YWt[ii*cnp+jj];
<a name="l01163"></a>01163 <span class="preprocessor">#endif</span>
<a name="l01164"></a>01164 <span class="preprocessor"></span>          }
<a name="l01165"></a>01165           <span class="keywordflow">else</span>{
<a name="l01166"></a>01166             ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01167"></a>01167 
<a name="l01168"></a>01168             <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii, ptr2+=Sdim)
<a name="l01169"></a>01169               <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj)
<a name="l01170"></a>01170                 ptr2[jj]=
<a name="l01171"></a>01171 #<span class="keywordflow">if</span> MAT_STORAGE==COLUMN_MAJOR
<a name="l01172"></a>01172                                                 ptr1[jj*cnp+ii] - YWt[jj*cnp+ii];
<a name="l01173"></a>01173 <span class="preprocessor">#else</span>
<a name="l01174"></a>01174 <span class="preprocessor"></span>                                                ptr1[ii*cnp+jj] - YWt[ii*cnp+jj];
<a name="l01175"></a>01175 <span class="preprocessor">#endif</span>
<a name="l01176"></a>01176 <span class="preprocessor"></span>          }
<a name="l01177"></a>01177         }
<a name="l01178"></a>01178 
<a name="l01179"></a>01179         <span class="comment">/* copy the LOWER TRIANGULAR PART of S from the upper one */</span>
<a name="l01180"></a>01180         <span class="keywordflow">for</span>(k=mcon; k&lt;j; ++k){
<a name="l01181"></a>01181 <span class="preprocessor">#if MAT_STORAGE==COLUMN_MAJOR</span>
<a name="l01182"></a>01182 <span class="preprocessor"></span>          ptr1=S + (k-mcon)*mmconxUsz + (j-mcon)*cnp; <span class="comment">// set ptr1 to point to the beginning of block j,k in S</span>
<a name="l01183"></a>01183           ptr2=S + (j-mcon)*mmconxUsz + (k-mcon)*cnp; <span class="comment">// set ptr2 to point to the beginning of block k,j in S</span>
<a name="l01184"></a>01184 <span class="preprocessor">#else</span>
<a name="l01185"></a>01185 <span class="preprocessor"></span>          ptr1=S + (j-mcon)*mmconxUsz + (k-mcon)*cnp; <span class="comment">// set ptr1 to point to the beginning of block j,k in S</span>
<a name="l01186"></a>01186           ptr2=S + (k-mcon)*mmconxUsz + (j-mcon)*cnp; <span class="comment">// set ptr2 to point to the beginning of block k,j in S</span>
<a name="l01187"></a>01187 <span class="preprocessor">#endif</span>
<a name="l01188"></a>01188 <span class="preprocessor"></span>          <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii, ptr1+=Sdim)
<a name="l01189"></a>01189             <span class="keywordflow">for</span>(jj=0, ptr3=ptr2+ii; jj&lt;cnp; ++jj, ptr3+=Sdim)
<a name="l01190"></a>01190               ptr1[jj]=*ptr3;
<a name="l01191"></a>01191         }
<a name="l01192"></a>01192 
<a name="l01193"></a>01193         <span class="comment">/* compute e_j=ea_j - \sum_i Y_ij eb_i */</span>
<a name="l01194"></a>01194         <span class="comment">/* Recall that Y_ij is cnp x pnp and eb_i is pnp x 1 */</span>
<a name="l01195"></a>01195         ptr1=E + j*easz; <span class="comment">// set ptr1 to point to e_j</span>
<a name="l01196"></a>01196 
<a name="l01197"></a>01197         <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l01198"></a>01198           <span class="comment">/* set ptr2 to point to Y_ij, actual row number in rcsubs[i] */</span>
<a name="l01199"></a>01199           ptr2=Yj + i*Ysz;
<a name="l01200"></a>01200 
<a name="l01201"></a>01201           <span class="comment">/* set ptr3 to point to eb_i */</span>
<a name="l01202"></a>01202           ptr3=eb + rcsubs[i]*ebsz;
<a name="l01203"></a>01203           <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii){
<a name="l01204"></a>01204             ptr4=ptr2+ii*pnp;
<a name="l01205"></a>01205             <span class="keywordflow">for</span>(jj=0, sum=0.0; jj&lt;pnp; ++jj)
<a name="l01206"></a>01206               sum+=ptr4[jj]*ptr3[jj]; <span class="comment">//ptr2[ii*pnp+jj]*ptr3[jj];</span>
<a name="l01207"></a>01207             ptr1[ii]+=sum;
<a name="l01208"></a>01208           }
<a name="l01209"></a>01209         }
<a name="l01210"></a>01210 
<a name="l01211"></a>01211         ptr2=ea + j*easz; <span class="comment">// set ptr2 to point to ea_j</span>
<a name="l01212"></a>01212         <span class="keywordflow">for</span>(i=0; i&lt;easz; ++i)
<a name="l01213"></a>01213           ptr1[i]=ptr2[i] - ptr1[i];
<a name="l01214"></a>01214       }
<a name="l01215"></a>01215 
<a name="l01216"></a>01216 <span class="preprocessor">#if 0</span>
<a name="l01217"></a>01217 <span class="preprocessor"></span>      <span class="keywordflow">if</span>(verbose&gt;1){ <span class="comment">/* count the nonzeros in S */</span>
<a name="l01218"></a>01218         <span class="keywordflow">for</span>(i=ii=0; i&lt;Sdim*Sdim; ++i)
<a name="l01219"></a>01219           <span class="keywordflow">if</span>(S[i]!=0.0) ++ii;
<a name="l01220"></a>01220         printf(<span class="stringliteral">&quot;\nS density: %.5g\n&quot;</span>, ((<span class="keywordtype">double</span>)ii)/(Sdim*Sdim)); fflush(stdout);
<a name="l01221"></a>01221       }
<a name="l01222"></a>01222 <span class="preprocessor">#endif</span>
<a name="l01223"></a>01223 <span class="preprocessor"></span>
<a name="l01224"></a>01224 <span class="comment">// -----------------------------</span>
<a name="l01225"></a>01225 <span class="preprocessor">        #ifdef SAVE_REDUCED_MATRIX</span>
<a name="l01226"></a>01226 <span class="preprocessor"></span>        {
<a name="l01227"></a>01227         dimS = m*cnp;
<a name="l01228"></a>01228         copyS = (<span class="keywordtype">double</span>*) malloc(<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>)*dimS*dimS);
<a name="l01229"></a>01229         <span class="keywordtype">int</span> i;
<a name="l01230"></a>01230         <span class="keywordflow">for</span>(i = 0; i &lt; dimS*dimS; i++)
<a name="l01231"></a>01231                 copyS[i] = S[i];
<a name="l01232"></a>01232         }
<a name="l01233"></a>01233 <span class="preprocessor">        #endif</span>
<a name="l01234"></a>01234 <span class="preprocessor"></span><span class="comment">// -----------------------------</span>
<a name="l01235"></a>01235 
<a name="l01236"></a>01236         <span class="comment">// Nota: aqui se resuelve el sistema lineal &#39;S xInc = E&#39;</span>
<a name="l01237"></a>01237 clock_t t0_solve = clock();
<a name="l01238"></a>01238       <span class="comment">/* solve the linear system S dpa = E to compute the da_j.</span>
<a name="l01239"></a>01239 <span class="comment">       *</span>
<a name="l01240"></a>01240 <span class="comment">       * Note that if MAT_STORAGE==1 S is modified in the following call;</span>
<a name="l01241"></a>01241 <span class="comment">       * this is OK since S is recomputed for each iteration</span>
<a name="l01242"></a>01242 <span class="comment">       */</span>
<a name="l01243"></a>01243             <span class="comment">//issolved=sba_Axb_LU(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_LU;</span>
<a name="l01244"></a>01244       issolved=sba_Axb_Chol(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_Chol;
<a name="l01245"></a>01245 clock_t t1_solve = clock();
<a name="l01246"></a>01246         time_solve += t1_solve - t0_solve;
<a name="l01247"></a>01247       <span class="comment">//issolved=sba_Axb_BK(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_BK;</span>
<a name="l01248"></a>01248       <span class="comment">//issolved=sba_Axb_QRnoQ(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_QRnoQ;</span>
<a name="l01249"></a>01249       <span class="comment">//issolved=sba_Axb_QR(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_QR;</span>
<a name="l01250"></a>01250             <span class="comment">//issolved=sba_Axb_SVD(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_SVD;</span>
<a name="l01251"></a>01251             <span class="comment">//issolved=sba_Axb_CG(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, (3*Sdim)/2, 1E-10, SBA_CG_JACOBI, MAT_STORAGE); linsolver=(PLS)sba_Axb_CG;</span>
<a name="l01252"></a>01252 
<a name="l01253"></a>01253       ++nlss;
<a name="l01254"></a>01254 
<a name="l01255"></a>01255             _dblzero(dpa, mcon*cnp); <span class="comment">/* no change for the first mcon camera params */</span>
<a name="l01256"></a>01256 
<a name="l01257"></a>01257       <span class="keywordflow">if</span>(issolved){
<a name="l01258"></a>01258 
<a name="l01259"></a>01259         <span class="comment">/* compute the db_i */</span>
<a name="l01260"></a>01260         <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l01261"></a>01261           ptr1=dpb + i*ebsz; <span class="comment">// set ptr1 to point to db_i</span>
<a name="l01262"></a>01262 
<a name="l01263"></a>01263           <span class="comment">/* compute \sum_j W_ij^T da_j */</span>
<a name="l01264"></a>01264           <span class="comment">/* Recall that W_ij is cnp x pnp and da_j is cnp x 1 */</span>
<a name="l01265"></a>01265           _dblzero(Wtda, Wtdasz); <span class="comment">/* clear Wtda */</span>
<a name="l01266"></a>01266           nnz=sba_crsm_row_elmidxs(&amp;idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero W_ij, j=0...m-1 */</span>
<a name="l01267"></a>01267           <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l01268"></a>01268             <span class="comment">/* set ptr2 to point to W_ij, actual column number in rcsubs[j] */</span>
<a name="l01269"></a>01269                               <span class="keywordflow">if</span>(rcsubs[j]&lt;mcon) <span class="keywordflow">continue</span>; <span class="comment">/* W_ij is zero */</span>
<a name="l01270"></a>01270 
<a name="l01271"></a>01271             ptr2=W + idxij.val[rcidxs[j]]*Wsz;
<a name="l01272"></a>01272 
<a name="l01273"></a>01273             <span class="comment">/* set ptr3 to point to da_j */</span>
<a name="l01274"></a>01274             ptr3=dpa + rcsubs[j]*cnp;
<a name="l01275"></a>01275 
<a name="l01276"></a>01276             <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii){
<a name="l01277"></a>01277               ptr4=ptr2+ii;
<a name="l01278"></a>01278               <span class="keywordflow">for</span>(jj=0, sum=0.0; jj&lt;cnp; ++jj)
<a name="l01279"></a>01279                 sum+=ptr4[jj*pnp]*ptr3[jj]; <span class="comment">//ptr2[jj*pnp+ii]*ptr3[jj];</span>
<a name="l01280"></a>01280               Wtda[ii]+=sum;
<a name="l01281"></a>01281             }
<a name="l01282"></a>01282           }
<a name="l01283"></a>01283 
<a name="l01284"></a>01284           <span class="comment">/* compute eb_i - \sum_j W_ij^T da_j = eb_i - Wtda in Wtda */</span>
<a name="l01285"></a>01285           ptr2=eb + i*ebsz; <span class="comment">// set ptr2 to point to eb_i</span>
<a name="l01286"></a>01286           <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii)
<a name="l01287"></a>01287             Wtda[ii]=ptr2[ii] - Wtda[ii];
<a name="l01288"></a>01288 
<a name="l01289"></a>01289           <span class="comment">/* compute the product (V*_i)^-1 Wtda = (V*_i)^-1 (eb_i - \sum_j W_ij^T da_j).</span>
<a name="l01290"></a>01290 <span class="comment">           * Recall that only the lower triangle of (V*_i)^-1 is stored</span>
<a name="l01291"></a>01291 <span class="comment">           */</span>
<a name="l01292"></a>01292           ptr2=V + i*Vsz; <span class="comment">// set ptr2 to point to (V*_i)^-1</span>
<a name="l01293"></a>01293           <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii){
<a name="l01294"></a>01294             <span class="keywordflow">for</span>(jj=0, sum=0.0; jj&lt;=ii; ++jj)
<a name="l01295"></a>01295               sum+=ptr2[ii*pnp+jj]*Wtda[jj];
<a name="l01296"></a>01296             <span class="keywordflow">for</span>( ; jj&lt;pnp; ++jj)
<a name="l01297"></a>01297               sum+=ptr2[jj*pnp+ii]*Wtda[jj];
<a name="l01298"></a>01298             ptr1[ii]=sum;
<a name="l01299"></a>01299           }
<a name="l01300"></a>01300         }
<a name="l01301"></a>01301               _dblzero(dpb, ncon*pnp); <span class="comment">/* no change for the first ncon point params */</span>
<a name="l01302"></a>01302 
<a name="l01303"></a>01303         <span class="comment">/* parameter vector updates are now in dpa, dpb */</span>
<a name="l01304"></a>01304 
<a name="l01305"></a>01305         <span class="comment">/* compute p&#39;s new estimate and ||dp||^2 */</span>
<a name="l01306"></a>01306         <span class="keywordflow">for</span>(i=0, dp_L2=0.0; i&lt;nvars; ++i){
<a name="l01307"></a>01307           pdp[i]=p[i] + (tmp=dp[i]);
<a name="l01308"></a>01308           dp_L2+=tmp*tmp;
<a name="l01309"></a>01309         }
<a name="l01310"></a>01310         <span class="comment">//dp_L2=sqrt(dp_L2);</span>
<a name="l01311"></a>01311 
<a name="l01312"></a>01312         <span class="keywordflow">if</span>(dp_L2&lt;=eps2_sq*p_L2){ <span class="comment">/* relative change in p is small, stop */</span>
<a name="l01313"></a>01313         <span class="comment">//if(dp_L2&lt;=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */</span>
<a name="l01314"></a>01314           stop=2;
<a name="l01315"></a>01315           <span class="keywordflow">break</span>;
<a name="l01316"></a>01316         }
<a name="l01317"></a>01317 
<a name="l01318"></a>01318         <span class="keywordflow">if</span>(dp_L2&gt;=(p_L2+eps2)/SBA_EPSILON_SQ){ <span class="comment">/* almost singular */</span>
<a name="l01319"></a>01319         <span class="comment">//if(dp_L2&gt;=(p_L2+eps2)/SBA_EPSILON){ /* almost singular */</span>
<a name="l01320"></a>01320           fprintf(stderr, <span class="stringliteral">&quot;SBA: the matrix of the augmented normal equations is almost singular in sba_motstr_levmar_x(),\n&quot;</span>
<a name="l01321"></a>01321                           <span class="stringliteral">&quot;     minimization should be restarted from the current solution with an increased damping term\n&quot;</span>);
<a name="l01322"></a>01322           retval=SBA_ERROR;
<a name="l01323"></a>01323           <span class="keywordflow">goto</span> freemem_and_return;
<a name="l01324"></a>01324        }
<a name="l01325"></a>01325 
<a name="l01326"></a>01326         (*func)(pdp, &amp;idxij, rcidxs, rcsubs, hx, adata); ++nfev; <span class="comment">/* evaluate function at p + dp */</span>
<a name="l01327"></a>01327         <span class="keywordflow">if</span>(verbose&gt;1)
<a name="l01328"></a>01328           printf(<span class="stringliteral">&quot;mean reprojection error %g\n&quot;</span>, sba_mean_repr_error(n, mnp, x, hx, &amp;idxij, rcidxs, rcsubs));
<a name="l01329"></a>01329         <span class="comment">/* ### compute ||e(pdp)||_2 */</span>
<a name="l01330"></a>01330         <span class="keywordflow">if</span>(covx==NULL)
<a name="l01331"></a>01331           pdp_eL2=nrmL2xmy(hx, x, hx, nobs); <span class="comment">/* hx=x-hx, pdp_eL2=||hx|| */</span>
<a name="l01332"></a>01332         <span class="keywordflow">else</span>
<a name="l01333"></a>01333           pdp_eL2=nrmCxmy(hx, x, hx, wght, mnp, nvis); <span class="comment">/* hx=wght*(x-hx), pdp_eL2=||hx|| */</span>
<a name="l01334"></a>01334         <span class="keywordflow">if</span>(!SBA_FINITE(pdp_eL2)){
<a name="l01335"></a>01335           <span class="keywordflow">if</span>(verbose) <span class="comment">/* identify the offending point projection */</span>
<a name="l01336"></a>01336             sba_print_inf(hx, m, mnp, &amp;idxij, rcidxs, rcsubs);
<a name="l01337"></a>01337 
<a name="l01338"></a>01338           stop=7;
<a name="l01339"></a>01339           <span class="keywordflow">break</span>;
<a name="l01340"></a>01340         }
<a name="l01341"></a>01341 
<a name="l01342"></a>01342         <span class="keywordflow">for</span>(i=0, dL=0.0; i&lt;nvars; ++i)
<a name="l01343"></a>01343           dL+=dp[i]*(mu*dp[i]+eab[i]);
<a name="l01344"></a>01344 
<a name="l01345"></a>01345         dF=p_eL2-pdp_eL2;
<a name="l01346"></a>01346 
<a name="l01347"></a>01347         <span class="keywordflow">if</span>(verbose&gt;1)
<a name="l01348"></a>01348           printf(<span class="stringliteral">&quot;\ndamping term %8g, gain ratio %8g, errors %8g / %8g = %g\n&quot;</span>, mu, dL!=0.0? dF/dL : dF/DBL_EPSILON, p_eL2/nvis, pdp_eL2/nvis, p_eL2/pdp_eL2);
<a name="l01349"></a>01349 
<a name="l01350"></a>01350         <span class="keywordflow">if</span>(dL&gt;0.0 &amp;&amp; dF&gt;0.0){ <span class="comment">/* reduction in error, increment is accepted */</span>
<a name="l01351"></a>01351           tmp=(2.0*dF/dL-1.0);
<a name="l01352"></a>01352           tmp=1.0-tmp*tmp*tmp;
<a name="l01353"></a>01353           mu=mu*( (tmp&gt;=SBA_ONE_THIRD)? tmp : SBA_ONE_THIRD );
<a name="l01354"></a>01354           nu=2;
<a name="l01355"></a>01355 
<a name="l01356"></a>01356           <span class="comment">/* the test below is equivalent to the relative reduction of the RMS reprojection error: sqrt(p_eL2)-sqrt(pdp_eL2)&lt;eps4*sqrt(p_eL2) */</span>
<a name="l01357"></a>01357           <span class="keywordflow">if</span>(pdp_eL2-2.0*sqrt(p_eL2*pdp_eL2)&lt;(eps4_sq-1.0)*p_eL2) stop=4;
<a name="l01358"></a>01358           
<a name="l01359"></a>01359           <span class="keywordflow">for</span>(i=0; i&lt;nvars; ++i) <span class="comment">/* update p&#39;s estimate */</span>
<a name="l01360"></a>01360             p[i]=pdp[i];
<a name="l01361"></a>01361 
<a name="l01362"></a>01362           <span class="keywordflow">for</span>(i=0; i&lt;nobs; ++i) <span class="comment">/* update e and ||e||_2 */</span>
<a name="l01363"></a>01363             e[i]=hx[i];
<a name="l01364"></a>01364           p_eL2=pdp_eL2;
<a name="l01365"></a>01365           <span class="keywordflow">break</span>;
<a name="l01366"></a>01366         }
<a name="l01367"></a>01367       } <span class="comment">/* issolved */</span>
<a name="l01368"></a>01368 
<a name="l01369"></a>01369 moredamping:
<a name="l01370"></a>01370       <span class="comment">/* if this point is reached (also via an explicit goto!), either the linear system could</span>
<a name="l01371"></a>01371 <span class="comment">       * not be solved or the error did not reduce; in any case, the increment must be rejected</span>
<a name="l01372"></a>01372 <span class="comment">       */</span>
<a name="l01373"></a>01373 
<a name="l01374"></a>01374       mu*=nu;
<a name="l01375"></a>01375       nu2=nu&lt;&lt;1; <span class="comment">// 2*nu;</span>
<a name="l01376"></a>01376       <span class="keywordflow">if</span>(nu2&lt;=nu){ <span class="comment">/* nu has wrapped around (overflown) */</span>
<a name="l01377"></a>01377         fprintf(stderr, <span class="stringliteral">&quot;SBA: too many failed attempts to increase the damping factor in sba_motstr_levmar_x()! Singular Hessian matrix?\n&quot;</span>);
<a name="l01378"></a>01378         <span class="comment">//exit(1);</span>
<a name="l01379"></a>01379         stop=6;
<a name="l01380"></a>01380         <span class="keywordflow">break</span>;
<a name="l01381"></a>01381       }
<a name="l01382"></a>01382       nu=nu2;
<a name="l01383"></a>01383 
<a name="l01384"></a>01384 <span class="preprocessor">#if 0</span>
<a name="l01385"></a>01385 <span class="preprocessor"></span>      <span class="comment">/* restore U, V diagonal entries */</span>
<a name="l01386"></a>01386       <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01387"></a>01387         ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01388"></a>01388         ptr2=diagU + j*cnp; <span class="comment">// set ptr2 to point to diagU_j</span>
<a name="l01389"></a>01389         <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01390"></a>01390           ptr1[i*cnp+i]=ptr2[i];
<a name="l01391"></a>01391       }
<a name="l01392"></a>01392       <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l01393"></a>01393         ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l01394"></a>01394         ptr2=diagV + i*pnp; <span class="comment">// set ptr2 to point to diagV_i</span>
<a name="l01395"></a>01395         <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l01396"></a>01396           ptr1[j*pnp+j]=ptr2[j];
<a name="l01397"></a>01397       }
<a name="l01398"></a>01398 <span class="preprocessor">#endif</span>
<a name="l01399"></a>01399 <span class="preprocessor"></span>    } <span class="comment">/* inner while loop */</span>
<a name="l01400"></a>01400         <span class="comment">// --------------------------------------------------------------------------------------------------------------------------------------------</span>
<a name="l01401"></a>01401         <span class="comment">// Nota: fin &#39;while&#39;</span>
<a name="l01402"></a>01402     <span class="keywordflow">if</span>(p_eL2&lt;=eps3_sq) stop=5; <span class="comment">// error is small, force termination of outer loop</span>
<a name="l01403"></a>01403   }
<a name="l01404"></a>01404 
<a name="l01405"></a>01405   <span class="keywordflow">if</span>(itno&gt;=itmax) stop=3;
<a name="l01406"></a>01406 
<a name="l01407"></a>01407   <span class="comment">/* restore U, V diagonal entries */</span>
<a name="l01408"></a>01408   <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01409"></a>01409     ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01410"></a>01410     ptr2=diagU + j*cnp; <span class="comment">// set ptr2 to point to diagU_j</span>
<a name="l01411"></a>01411     <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01412"></a>01412       ptr1[i*cnp+i]=ptr2[i];
<a name="l01413"></a>01413   }
<a name="l01414"></a>01414   <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l01415"></a>01415     ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l01416"></a>01416     ptr2=diagV + i*pnp; <span class="comment">// set ptr2 to point to diagV_i</span>
<a name="l01417"></a>01417     <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l01418"></a>01418      ptr1[j*pnp+j]=ptr2[j];
<a name="l01419"></a>01419   }
<a name="l01420"></a>01420 
<a name="l01421"></a>01421   <span class="keywordflow">if</span>(info){
<a name="l01422"></a>01422     info[0]=init_p_eL2;
<a name="l01423"></a>01423     info[1]=p_eL2;
<a name="l01424"></a>01424     info[2]=eab_inf;
<a name="l01425"></a>01425     info[3]=dp_L2;
<a name="l01426"></a>01426     <span class="keywordflow">for</span>(j=mcon, tmp=DBL_MIN; j&lt;m; ++j){
<a name="l01427"></a>01427       ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01428"></a>01428       <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01429"></a>01429         <span class="keywordflow">if</span>(tmp&lt;ptr1[i*cnp+i]) tmp=ptr1[i*cnp+i];
<a name="l01430"></a>01430     }
<a name="l01431"></a>01431     <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l01432"></a>01432       ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l01433"></a>01433       <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l01434"></a>01434         <span class="keywordflow">if</span>(tmp&lt;ptr1[j*pnp+j]) tmp=ptr1[j*pnp+j];
<a name="l01435"></a>01435       }
<a name="l01436"></a>01436     info[4]=mu/tmp;
<a name="l01437"></a>01437     info[5]=itno;
<a name="l01438"></a>01438     info[6]=stop;
<a name="l01439"></a>01439     info[7]=nfev;
<a name="l01440"></a>01440     info[8]=njev;
<a name="l01441"></a>01441     info[9]=nlss;
<a name="l01442"></a>01442   }
<a name="l01443"></a>01443                                                                
<a name="l01444"></a>01444   <span class="comment">//sba_print_sol(n, m, p, cnp, pnp, x, mnp, &amp;idxij, rcidxs, rcsubs);</span>
<a name="l01445"></a>01445   retval=(stop!=7)?  itno : SBA_ERROR;
<a name="l01446"></a>01446 
<a name="l01447"></a>01447 freemem_and_return: <span class="comment">/* NOTE: this point is also reached via a goto! */</span>
<a name="l01448"></a>01448 
<a name="l01449"></a>01449    <span class="comment">/* free whatever was allocated */</span>
<a name="l01450"></a>01450   free(W);   free(U);  free(V);
<a name="l01451"></a>01451   free(e);   free(eab);
<a name="l01452"></a>01452   free(E);   free(Yj); free(YWt);
<a name="l01453"></a>01453   free(S);   free(dp); free(Wtda);
<a name="l01454"></a>01454 <span class="comment">// Nota: no destruimos &#39;S&#39;, sino que la guardamos</span>
<a name="l01455"></a>01455   free(rcidxs); free(rcsubs);
<a name="l01456"></a>01456 <span class="preprocessor">#ifndef SBA_DESTROY_COVS</span>
<a name="l01457"></a>01457 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(wght) free(wght);
<a name="l01458"></a>01458 <span class="preprocessor">#else</span>
<a name="l01459"></a>01459 <span class="preprocessor"></span>  <span class="comment">/* nothing to do */</span>
<a name="l01460"></a>01460 <span class="preprocessor">#endif </span><span class="comment">/* SBA_DESTROY_COVS */</span>
<a name="l01461"></a>01461 
<a name="l01462"></a>01462   free(hx); free(diagUV); free(pdp);
<a name="l01463"></a>01463   <span class="keywordflow">if</span>(fdj_data.hxx){ <span class="comment">// cleanup</span>
<a name="l01464"></a>01464     free(fdj_data.hxx);
<a name="l01465"></a>01465     free(fdj_data.func_rcidxs);
<a name="l01466"></a>01466   }
<a name="l01467"></a>01467 
<a name="l01468"></a>01468   sba_crsm_free(&amp;idxij);
<a name="l01469"></a>01469 
<a name="l01470"></a>01470   <span class="comment">/* free the memory allocated by the matrix inversion &amp; linear solver routines */</span>
<a name="l01471"></a>01471   <span class="keywordflow">if</span>(matinv) (*matinv)(NULL, 0);
<a name="l01472"></a>01472   <span class="keywordflow">if</span>(linsolver) (*linsolver)(NULL, NULL, NULL, 0, 0);
<a name="l01473"></a>01473 
<a name="l01474"></a>01474   clock_t t_end = clock();
<a name="l01475"></a>01475 
<a name="l01476"></a>01476 time_total = t_end - t_init;
<a name="l01477"></a>01477 
<a name="l01478"></a>01478 sba_time_total = (double)time_total / CLOCKS_PER_SEC,
<a name="l01479"></a>01479 sba_time_solve = (<span class="keywordtype">double</span>)time_solve / CLOCKS_PER_SEC;
<a name="l01480"></a>01480 sba_time_system = sba_time_total - sba_time_solve;
<a name="l01481"></a>01481 
<a name="l01482"></a>01482 printf(<span class="stringliteral">&quot;**** time total = %f\n&quot;</span>, sba_time_total);
<a name="l01483"></a>01483 printf(<span class="stringliteral">&quot;**** time solve = %f\n&quot;</span>, sba_time_solve);
<a name="l01484"></a>01484 printf(<span class="stringliteral">&quot;**** time not solve = %f\n&quot;</span>, sba_time_system);
<a name="l01485"></a>01485 printf(<span class="stringliteral">&quot;**** mu = %.12f\n&quot;</span>, mu);
<a name="l01486"></a>01486 last_mu_used = mu;
<a name="l01487"></a>01487   <span class="keywordflow">return</span> retval;
<a name="l01488"></a>01488 }
<a name="l01489"></a>01489 
<a name="l01490"></a>01490 <span class="comment">// ==========================================================================================================================================</span>
<a name="l01491"></a>01491 
<a name="l01492"></a>01492 <span class="comment">/* Bundle adjustment on camera parameters only </span>
<a name="l01493"></a>01493 <span class="comment"> * using the sparse Levenberg-Marquardt as described in HZ p. 568</span>
<a name="l01494"></a>01494 <span class="comment"> *</span>
<a name="l01495"></a>01495 <span class="comment"> * Returns the number of iterations (&gt;=0) if successfull, SBA_ERROR if failed</span>
<a name="l01496"></a>01496 <span class="comment"> */</span>
<a name="l01497"></a>01497 
<a name="l01498"></a>01498 <span class="keywordtype">int</span> sba_mot_levmar_x(
<a name="l01499"></a>01499     <span class="keyword">const</span> <span class="keywordtype">int</span> n,   <span class="comment">/* number of points */</span>
<a name="l01500"></a>01500     <span class="keyword">const</span> <span class="keywordtype">int</span> m,   <span class="comment">/* number of images */</span>
<a name="l01501"></a>01501     <span class="keyword">const</span> <span class="keywordtype">int</span> mcon,<span class="comment">/* number of images (starting from the 1st) whose parameters should not be modified.</span>
<a name="l01502"></a>01502 <span class="comment">                                                  * All A_ij (see below) with j&lt;mcon are assumed to be zero</span>
<a name="l01503"></a>01503 <span class="comment">                                                  */</span>
<a name="l01504"></a>01504     <span class="keywordtype">char</span> *vmask,  <span class="comment">/* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */</span>
<a name="l01505"></a>01505     <span class="keywordtype">double</span> *p,    <span class="comment">/* initial parameter vector p0: (a1, ..., am).</span>
<a name="l01506"></a>01506 <span class="comment">                   * aj are the image j parameters, size m*cnp */</span>
<a name="l01507"></a>01507     <span class="keyword">const</span> <span class="keywordtype">int</span> cnp,<span class="comment">/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */</span>
<a name="l01508"></a>01508     <span class="keywordtype">double</span> *x,    <span class="comment">/* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where</span>
<a name="l01509"></a>01509 <span class="comment">                   * x_ij is the projection of the i-th point on the j-th image.</span>
<a name="l01510"></a>01510 <span class="comment">                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;</span>
<a name="l01511"></a>01511 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp</span>
<a name="l01512"></a>01512 <span class="comment">                   */</span>
<a name="l01513"></a>01513     <span class="keywordtype">double</span> *covx, <span class="comment">/* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm),</span>
<a name="l01514"></a>01514 <span class="comment">                   * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no</span>
<a name="l01515"></a>01515 <span class="comment">                   * covariance estimates are available (identity matrices are implicitly used in this case).</span>
<a name="l01516"></a>01516 <span class="comment">                   * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing;</span>
<a name="l01517"></a>01517 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp*mnp</span>
<a name="l01518"></a>01518 <span class="comment">                   */</span>
<a name="l01519"></a>01519     <span class="keyword">const</span> <span class="keywordtype">int</span> mnp,<span class="comment">/* number of parameters for EACH measurement; usually 2 */</span>
<a name="l01520"></a>01520     <span class="keywordtype">void</span> (*func)(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata),
<a name="l01521"></a>01521                                               <span class="comment">/* functional relation describing measurements. Given a parameter vector p,</span>
<a name="l01522"></a>01522 <span class="comment">                                               * computes a prediction of the measurements \hat{x}. p is (m*cnp)x1,</span>
<a name="l01523"></a>01523 <span class="comment">                                               * \hat{x} is (n*m*mnp)x1, maximum</span>
<a name="l01524"></a>01524 <span class="comment">                                               * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used</span>
<a name="l01525"></a>01525 <span class="comment">                                               * as working memory</span>
<a name="l01526"></a>01526 <span class="comment">                                               */</span>
<a name="l01527"></a>01527     <span class="keywordtype">void</span> (*fjac)(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata),
<a name="l01528"></a>01528                                               <span class="comment">/* function to evaluate the sparse jacobian dX/dp.</span>
<a name="l01529"></a>01529 <span class="comment">                                               * The Jacobian is returned in jac as</span>
<a name="l01530"></a>01530 <span class="comment">                                               * (dx_11/da_1, ..., dx_1m/da_m, ..., dx_n1/da_1, ..., dx_nm/da_m), or (using HZ&#39;s notation),</span>
<a name="l01531"></a>01531 <span class="comment">                                               * jac=(A_11, ..., A_1m, ..., A_n1, ..., A_nm)</span>
<a name="l01532"></a>01532 <span class="comment">                                               * Notice that depending on idxij, some of the A_ij might be missing.</span>
<a name="l01533"></a>01533 <span class="comment">                                               * Note also that the A_ij are mnp x cnp matrices and they</span>
<a name="l01534"></a>01534 <span class="comment">                                               * should be stored in jac in row-major order.</span>
<a name="l01535"></a>01535 <span class="comment">                                               * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used</span>
<a name="l01536"></a>01536 <span class="comment">                                               * as working memory</span>
<a name="l01537"></a>01537 <span class="comment">                                               *</span>
<a name="l01538"></a>01538 <span class="comment">                                               * If NULL, the jacobian is approximated by repetitive func calls and finite</span>
<a name="l01539"></a>01539 <span class="comment">                                               * differences. This is computationally inefficient and thus NOT recommended.</span>
<a name="l01540"></a>01540 <span class="comment">                                               */</span>
<a name="l01541"></a>01541     <span class="keywordtype">void</span> *adata,       <span class="comment">/* pointer to possibly additional data, passed uninterpreted to func, fjac */</span> 
<a name="l01542"></a>01542 
<a name="l01543"></a>01543     <span class="keyword">const</span> <span class="keywordtype">int</span> itmax,   <span class="comment">/* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */</span>
<a name="l01544"></a>01544     <span class="keyword">const</span> <span class="keywordtype">int</span> verbose, <span class="comment">/* I: verbosity */</span>
<a name="l01545"></a>01545     <span class="keyword">const</span> <span class="keywordtype">double</span> opts[SBA_OPTSSZ],
<a name="l01546"></a>01546                              <span class="comment">/* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu,</span>
<a name="l01547"></a>01547 <span class="comment">                        * stopping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2</span>
<a name="l01548"></a>01548 <span class="comment">                        */</span>
<a name="l01549"></a>01549     <span class="keywordtype">double</span> info[SBA_INFOSZ]
<a name="l01550"></a>01550                              <span class="comment">/* O: information regarding the minimization. Set to NULL if don&#39;t care</span>
<a name="l01551"></a>01551 <span class="comment">                        * info[0]=||e||_2 at initial p.</span>
<a name="l01552"></a>01552 <span class="comment">                        * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.</span>
<a name="l01553"></a>01553 <span class="comment">                        * info[5]= # iterations,</span>
<a name="l01554"></a>01554 <span class="comment">                        * info[6]=reason for terminating: 1 - stopped by small gradient J^T e</span>
<a name="l01555"></a>01555 <span class="comment">                        *                                 2 - stopped by small dp</span>
<a name="l01556"></a>01556 <span class="comment">                        *                                 3 - stopped by itmax</span>
<a name="l01557"></a>01557 <span class="comment">                        *                                 4 - stopped by small relative reduction in ||e||_2</span>
<a name="l01558"></a>01558 <span class="comment">                        *                                 5 - stopped by small ||e||_2</span>
<a name="l01559"></a>01559 <span class="comment">                        *                                 6 - too many attempts to increase damping. Restart with increased mu</span>
<a name="l01560"></a>01560 <span class="comment">                        *                                 7 - stopped by invalid (i.e. NaN or Inf) &quot;func&quot; values. This is a user error</span>
<a name="l01561"></a>01561 <span class="comment">                        * info[7]= # function evaluations</span>
<a name="l01562"></a>01562 <span class="comment">                        * info[8]= # jacobian evaluations</span>
<a name="l01563"></a>01563 <span class="comment">                                          * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error</span>
<a name="l01564"></a>01564 <span class="comment">                        */</span>
<a name="l01565"></a>01565 )
<a name="l01566"></a>01566 {
<a name="l01567"></a>01567 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii, jj, k;
<a name="l01568"></a>01568 <span class="keywordtype">int</span> nvis, nnz, retval;
<a name="l01569"></a>01569 
<a name="l01570"></a>01570 <span class="comment">/* The following are work arrays that are dynamically allocated by sba_mot_levmar_x() */</span>
<a name="l01571"></a>01571 <span class="keywordtype">double</span> *jac; <span class="comment">/* work array for storing the jacobian, max. size n*m*mnp*cnp */</span>
<a name="l01572"></a>01572 <span class="keywordtype">double</span> *U;    <span class="comment">/* work array for storing the U_j in the order U_1, ..., U_m, size m*cnp*cnp */</span>
<a name="l01573"></a>01573 
<a name="l01574"></a>01574 <span class="keywordtype">double</span> *e;    <span class="comment">/* work array for storing the e_ij in the order e_11, ..., e_1m, ..., e_n1, ..., e_nm,</span>
<a name="l01575"></a>01575 <span class="comment">                 max. size n*m*mnp */</span>
<a name="l01576"></a>01576 <span class="keywordtype">double</span> *ea;   <span class="comment">/* work array for storing the ea_j in the order ea_1, .. ea_m, size m*cnp */</span>
<a name="l01577"></a>01577 
<a name="l01578"></a>01578 <span class="keywordtype">double</span> *dp;   <span class="comment">/* work array for storing the parameter vector updates da_1, ..., da_m, size m*cnp */</span>
<a name="l01579"></a>01579 
<a name="l01580"></a>01580 <span class="keywordtype">double</span> *wght= <span class="comment">/* work array for storing the weights computed from the covariance inverses, max. size n*m*mnp*mnp */</span>
<a name="l01581"></a>01581             NULL;
<a name="l01582"></a>01582 
<a name="l01583"></a>01583 <span class="comment">/* Of the above arrays, jac, e, wght are sparse and</span>
<a name="l01584"></a>01584 <span class="comment"> * U, ea, dp are dense. Sparse arrays are indexed through</span>
<a name="l01585"></a>01585 <span class="comment"> * idxij (see below), that is with the same mechanism as the input </span>
<a name="l01586"></a>01586 <span class="comment"> * measurements vector x</span>
<a name="l01587"></a>01587 <span class="comment"> */</span>
<a name="l01588"></a>01588 
<a name="l01589"></a>01589 <span class="comment">/* submatrices sizes */</span>
<a name="l01590"></a>01590 <span class="keywordtype">int</span> Asz, Usz,
<a name="l01591"></a>01591     esz, easz, covsz;
<a name="l01592"></a>01592 
<a name="l01593"></a>01593 <span class="keyword">register</span> <span class="keywordtype">double</span> *ptr1, *ptr2, *ptr3, *ptr4, sum;
<a name="l01594"></a>01594 <span class="keyword">struct </span>sba_crsm idxij; <span class="comment">/* sparse matrix containing the location of x_ij in x. This is also the location of A_ij </span>
<a name="l01595"></a>01595 <span class="comment">                        * in jac, e_ij in e, etc.</span>
<a name="l01596"></a>01596 <span class="comment">                        * This matrix can be thought as a map from a sparse set of pairs (i, j) to a continuous</span>
<a name="l01597"></a>01597 <span class="comment">                        * index k and it is used to efficiently lookup the memory locations where the non-zero</span>
<a name="l01598"></a>01598 <span class="comment">                        * blocks of a sparse matrix/vector are stored</span>
<a name="l01599"></a>01599 <span class="comment">                        */</span>
<a name="l01600"></a>01600 <span class="keywordtype">int</span> maxCPvis, <span class="comment">/* max. of projections across cameras &amp; projections across points */</span>
<a name="l01601"></a>01601     *rcidxs,  <span class="comment">/* work array for the indexes corresponding to the nonzero elements of a single row or</span>
<a name="l01602"></a>01602 <span class="comment">                 column in a sparse matrix, size max(n, m) */</span>
<a name="l01603"></a>01603     *rcsubs;  <span class="comment">/* work array for the subscripts of nonzero elements in a single row or column of a</span>
<a name="l01604"></a>01604 <span class="comment">                 sparse matrix, size max(n, m) */</span>
<a name="l01605"></a>01605 
<a name="l01606"></a>01606 <span class="comment">/* The following variables are needed by the LM algorithm */</span>
<a name="l01607"></a>01607 <span class="keyword">register</span> <span class="keywordtype">int</span> itno;  <span class="comment">/* iteration counter */</span>
<a name="l01608"></a>01608 <span class="keywordtype">int</span> nsolved;
<a name="l01609"></a>01609 <span class="comment">/* temporary work arrays that are dynamically allocated */</span>
<a name="l01610"></a>01610 <span class="keywordtype">double</span> *hx,         <span class="comment">/* \hat{x}_i, max. size m*n*mnp */</span>
<a name="l01611"></a>01611        *diagU,      <span class="comment">/* diagonals of U_j, size m*cnp */</span>
<a name="l01612"></a>01612        *pdp;        <span class="comment">/* p + dp, size m*cnp */</span>
<a name="l01613"></a>01613 
<a name="l01614"></a>01614 <span class="keyword">register</span> <span class="keywordtype">double</span> mu,  <span class="comment">/* damping constant */</span>
<a name="l01615"></a>01615                 tmp; <span class="comment">/* mainly used in matrix &amp; vector multiplications */</span>
<a name="l01616"></a>01616 <span class="keywordtype">double</span> p_eL2, ea_inf, pdp_eL2; <span class="comment">/* ||e(p)||_2, ||J^T e||_inf, ||e(p+dp)||_2 */</span>
<a name="l01617"></a>01617 <span class="keywordtype">double</span> p_L2, dp_L2=DBL_MAX, dF, dL;
<a name="l01618"></a>01618 <span class="keywordtype">double</span> tau=FABS(opts[0]), eps1=FABS(opts[1]), eps2=FABS(opts[2]), eps2_sq=opts[2]*opts[2],
<a name="l01619"></a>01619        eps3_sq=opts[3]*opts[3], eps4_sq=opts[4]*opts[4];
<a name="l01620"></a>01620 <span class="keywordtype">double</span> init_p_eL2;
<a name="l01621"></a>01621 <span class="keywordtype">int</span> nu=2, nu2, stop=0, nfev, njev=0, nlss=0;
<a name="l01622"></a>01622 <span class="keywordtype">int</span> nobs, nvars;
<a name="l01623"></a>01623 PLS linsolver=NULL;
<a name="l01624"></a>01624 
<a name="l01625"></a>01625 <span class="keyword">struct </span>fdj_data_x_ fdj_data;
<a name="l01626"></a>01626 <span class="keywordtype">void</span> *jac_adata;
<a name="l01627"></a>01627 
<a name="l01628"></a>01628 <span class="comment">/* Initialization */</span>
<a name="l01629"></a>01629   mu=ea_inf=0.0; <span class="comment">/* -Wall */</span>
<a name="l01630"></a>01630 
<a name="l01631"></a>01631   <span class="comment">/* block sizes */</span>
<a name="l01632"></a>01632   Asz=mnp * cnp; Usz=cnp * cnp;
<a name="l01633"></a>01633   esz=mnp; easz=cnp;
<a name="l01634"></a>01634   covsz=mnp * mnp;
<a name="l01635"></a>01635   
<a name="l01636"></a>01636   <span class="comment">/* count total number of visible image points */</span>
<a name="l01637"></a>01637   <span class="keywordflow">for</span>(i=nvis=0, jj=n*m; i&lt;jj; ++i)
<a name="l01638"></a>01638     nvis+=(vmask[i]!=0);
<a name="l01639"></a>01639 
<a name="l01640"></a>01640   nobs=nvis*mnp;
<a name="l01641"></a>01641   nvars=m*cnp;
<a name="l01642"></a>01642   <span class="keywordflow">if</span>(nobs&lt;nvars){
<a name="l01643"></a>01643     fprintf(stderr, <span class="stringliteral">&quot;SBA: sba_mot_levmar_x() cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n&quot;</span>, nobs, nvars);
<a name="l01644"></a>01644     <span class="keywordflow">return</span> SBA_ERROR;
<a name="l01645"></a>01645   }
<a name="l01646"></a>01646 
<a name="l01647"></a>01647   <span class="comment">/* allocate &amp; fill up the idxij structure */</span>
<a name="l01648"></a>01648   sba_crsm_alloc(&amp;idxij, n, m, nvis);
<a name="l01649"></a>01649   <span class="keywordflow">for</span>(i=k=0; i&lt;n; ++i){
<a name="l01650"></a>01650     idxij.rowptr[i]=k;
<a name="l01651"></a>01651     ii=i*m;
<a name="l01652"></a>01652     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l01653"></a>01653       <span class="keywordflow">if</span>(vmask[ii+j]){
<a name="l01654"></a>01654         idxij.val[k]=k;
<a name="l01655"></a>01655         idxij.colidx[k++]=j;
<a name="l01656"></a>01656       }
<a name="l01657"></a>01657   }
<a name="l01658"></a>01658   idxij.rowptr[n]=nvis;
<a name="l01659"></a>01659 
<a name="l01660"></a>01660   <span class="comment">/* find the maximum number of visible image points in any single camera or coming from a single 3D point */</span>
<a name="l01661"></a>01661   <span class="comment">/* cameras */</span>
<a name="l01662"></a>01662   <span class="keywordflow">for</span>(i=maxCPvis=0; i&lt;n; ++i)
<a name="l01663"></a>01663     <span class="keywordflow">if</span>((k=idxij.rowptr[i+1]-idxij.rowptr[i])&gt;maxCPvis) maxCPvis=k;
<a name="l01664"></a>01664 
<a name="l01665"></a>01665   <span class="comment">/* points, note that maxCPvis is not reinitialized! */</span>
<a name="l01666"></a>01666   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l01667"></a>01667     <span class="keywordflow">for</span>(i=ii=0; i&lt;n; ++i)
<a name="l01668"></a>01668       <span class="keywordflow">if</span>(vmask[i*m+j]) ++ii;
<a name="l01669"></a>01669     <span class="keywordflow">if</span>(ii&gt;maxCPvis) maxCPvis=ii;
<a name="l01670"></a>01670   }
<a name="l01671"></a>01671 
<a name="l01672"></a>01672   <span class="comment">/* allocate work arrays */</span>
<a name="l01673"></a>01673   jac=(<span class="keywordtype">double</span> *)emalloc(nvis*Asz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01674"></a>01674   U=(<span class="keywordtype">double</span> *)emalloc(m*Usz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01675"></a>01675   e=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01676"></a>01676   ea=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01677"></a>01677   dp=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01678"></a>01678   rcidxs=(<span class="keywordtype">int</span> *)emalloc(maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l01679"></a>01679   rcsubs=(<span class="keywordtype">int</span> *)emalloc(maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l01680"></a>01680 <span class="preprocessor">#ifndef SBA_DESTROY_COVS</span>
<a name="l01681"></a>01681 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(covx!=NULL) wght=(<span class="keywordtype">double</span> *)emalloc(nvis*covsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01682"></a>01682 <span class="preprocessor">#else</span>
<a name="l01683"></a>01683 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(covx!=NULL) wght=covx;
<a name="l01684"></a>01684 <span class="preprocessor">#endif </span><span class="comment">/* SBA_DESTROY_COVS */</span>
<a name="l01685"></a>01685 
<a name="l01686"></a>01686 
<a name="l01687"></a>01687   hx=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01688"></a>01688   diagU=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01689"></a>01689   pdp=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01690"></a>01690 
<a name="l01691"></a>01691   <span class="comment">/* if no jacobian function is supplied, prepare to compute jacobian with finite difference */</span>
<a name="l01692"></a>01692   <span class="keywordflow">if</span>(!fjac){
<a name="l01693"></a>01693     fdj_data.func=func;
<a name="l01694"></a>01694     fdj_data.cnp=cnp;
<a name="l01695"></a>01695     fdj_data.pnp=0;
<a name="l01696"></a>01696     fdj_data.mnp=mnp;
<a name="l01697"></a>01697     fdj_data.hx=hx;
<a name="l01698"></a>01698     fdj_data.hxx=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01699"></a>01699     fdj_data.func_rcidxs=(<span class="keywordtype">int</span> *)emalloc(2*maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l01700"></a>01700     fdj_data.func_rcsubs=fdj_data.func_rcidxs+maxCPvis;
<a name="l01701"></a>01701     fdj_data.adata=adata;
<a name="l01702"></a>01702 
<a name="l01703"></a>01703     fjac=sba_fdjac_x;
<a name="l01704"></a>01704     jac_adata=(<span class="keywordtype">void</span> *)&amp;fdj_data;
<a name="l01705"></a>01705   }
<a name="l01706"></a>01706   <span class="keywordflow">else</span>{
<a name="l01707"></a>01707     fdj_data.hxx=NULL;
<a name="l01708"></a>01708     jac_adata=adata;
<a name="l01709"></a>01709   }
<a name="l01710"></a>01710 
<a name="l01711"></a>01711   <span class="keywordflow">if</span>(itmax==0){ <span class="comment">/* verify jacobian */</span>
<a name="l01712"></a>01712     sba_mot_chkjac_x(func, fjac, p, &amp;idxij, rcidxs, rcsubs, mcon, cnp, mnp, adata, jac_adata);
<a name="l01713"></a>01713     retval=0;
<a name="l01714"></a>01714     <span class="keywordflow">goto</span> freemem_and_return;
<a name="l01715"></a>01715   }
<a name="l01716"></a>01716 
<a name="l01717"></a>01717   <span class="comment">/* covariances Sigma_x_ij are accommodated by computing the Cholesky decompositions of their</span>
<a name="l01718"></a>01718 <span class="comment">   * inverses and using the resulting matrices w_x_ij to weigh A_ij and e_ij as w_x_ij A_ij</span>
<a name="l01719"></a>01719 <span class="comment">   * and w_x_ij*e_ij. In this way, auxiliary variables as U_j=\sum_i A_ij^T A_ij</span>
<a name="l01720"></a>01720 <span class="comment">   * actually become \sum_i (w_x_ij A_ij)^T w_x_ij A_ij= \sum_i A_ij^T w_x_ij^T w_x_ij A_ij =</span>
<a name="l01721"></a>01721 <span class="comment">   * A_ij^T Sigma_x_ij^-1 A_ij</span>
<a name="l01722"></a>01722 <span class="comment">   *</span>
<a name="l01723"></a>01723 <span class="comment">   * ea_j are weighted in a similar manner</span>
<a name="l01724"></a>01724 <span class="comment">   */</span>
<a name="l01725"></a>01725   <span class="keywordflow">if</span>(covx!=NULL){
<a name="l01726"></a>01726     <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l01727"></a>01727       nnz=sba_crsm_row_elmidxs(&amp;idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero x_ij, j=0...m-1 */</span>
<a name="l01728"></a>01728       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l01729"></a>01729         <span class="comment">/* set ptr1, ptr2 to point to cov_x_ij, w_x_ij resp. */</span>
<a name="l01730"></a>01730         ptr1=covx + (k=idxij.val[rcidxs[j]]*covsz);
<a name="l01731"></a>01731         ptr2=wght + k;
<a name="l01732"></a>01732         <span class="keywordflow">if</span>(!sba_mat_cholinv(ptr1, ptr2, mnp)){ <span class="comment">/* compute w_x_ij s.t. w_x_ij^T w_x_ij = cov_x_ij^-1 */</span>
<a name="l01733"></a>01733                             fprintf(stderr, <span class="stringliteral">&quot;SBA: invalid covariance matrix for x_ij (i=%d, j=%d) in sba_motstr_levmar_x()\n&quot;</span>, i, rcsubs[j]);
<a name="l01734"></a>01734           retval=SBA_ERROR;
<a name="l01735"></a>01735           <span class="keywordflow">goto</span> freemem_and_return;
<a name="l01736"></a>01736         }
<a name="l01737"></a>01737       }
<a name="l01738"></a>01738     }
<a name="l01739"></a>01739     sba_mat_cholinv(NULL, NULL, 0); <span class="comment">/* cleanup */</span>
<a name="l01740"></a>01740   }
<a name="l01741"></a>01741 
<a name="l01742"></a>01742   <span class="comment">/* compute the error vectors e_ij in hx */</span>
<a name="l01743"></a>01743   (*func)(p, &amp;idxij, rcidxs, rcsubs, hx, adata); nfev=1;
<a name="l01744"></a>01744   <span class="comment">/* ### compute e=x - f(p) [e=w*(x - f(p)] and its L2 norm */</span>
<a name="l01745"></a>01745   <span class="keywordflow">if</span>(covx==NULL)
<a name="l01746"></a>01746     p_eL2=nrmL2xmy(e, x, hx, nobs); <span class="comment">/* e=x-hx, p_eL2=||e|| */</span>
<a name="l01747"></a>01747   <span class="keywordflow">else</span>
<a name="l01748"></a>01748     p_eL2=nrmCxmy(e, x, hx, wght, mnp, nvis); <span class="comment">/* e=wght*(x-hx), p_eL2=||e||=||x-hx||_Sigma^-1 */</span>
<a name="l01749"></a>01749   <span class="keywordflow">if</span>(verbose) printf(<span class="stringliteral">&quot;initial mot-SBA error %g [%g]\n&quot;</span>, p_eL2, p_eL2/nvis);
<a name="l01750"></a>01750   init_p_eL2=p_eL2;
<a name="l01751"></a>01751   <span class="keywordflow">if</span>(!SBA_FINITE(p_eL2)) stop=7;
<a name="l01752"></a>01752 
<a name="l01753"></a>01753   <span class="keywordflow">for</span>(itno=0; itno&lt;itmax &amp;&amp; !stop; ++itno){
<a name="l01754"></a>01754     <span class="comment">/* Note that p, e and ||e||_2 have been updated at the previous iteration */</span>
<a name="l01755"></a>01755 
<a name="l01756"></a>01756     <span class="comment">/* compute derivative submatrices A_ij */</span>
<a name="l01757"></a>01757     (*fjac)(p, &amp;idxij, rcidxs, rcsubs, jac, jac_adata); ++njev;
<a name="l01758"></a>01758 
<a name="l01759"></a>01759     <span class="keywordflow">if</span>(covx!=NULL){
<a name="l01760"></a>01760       <span class="comment">/* compute w_x_ij A_ij</span>
<a name="l01761"></a>01761 <span class="comment">       * Since w_x_ij is upper triangular, the products can be safely saved</span>
<a name="l01762"></a>01762 <span class="comment">       * directly in A_ij, without the need for intermediate storage</span>
<a name="l01763"></a>01763 <span class="comment">       */</span>
<a name="l01764"></a>01764       <span class="keywordflow">for</span>(i=0; i&lt;nvis; ++i){
<a name="l01765"></a>01765         <span class="comment">/* set ptr1, ptr2 to point to w_x_ij, A_ij, resp. */</span>
<a name="l01766"></a>01766         ptr1=wght + i*covsz;
<a name="l01767"></a>01767         ptr2=jac  + i*Asz;
<a name="l01768"></a>01768 
<a name="l01769"></a>01769         <span class="comment">/* w_x_ij is mnp x mnp, A_ij is mnp x cnp */</span>
<a name="l01770"></a>01770         <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l01771"></a>01771           <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj){
<a name="l01772"></a>01772             <span class="keywordflow">for</span>(k=ii, sum=0.0; k&lt;mnp; ++k) <span class="comment">// k&gt;=ii since w_x_ij is upper triangular</span>
<a name="l01773"></a>01773               sum+=ptr1[ii*mnp+k]*ptr2[k*cnp+jj];
<a name="l01774"></a>01774             ptr2[ii*cnp+jj]=sum;
<a name="l01775"></a>01775           }
<a name="l01776"></a>01776       }
<a name="l01777"></a>01777     }
<a name="l01778"></a>01778 
<a name="l01779"></a>01779     <span class="comment">/* compute U_j = \sum_i A_ij^T A_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l01780"></a>01780     <span class="comment">/* U_j is symmetric, therefore its computation can be sped up by</span>
<a name="l01781"></a>01781 <span class="comment">     * computing only the upper part and then reusing it for the lower one.</span>
<a name="l01782"></a>01782 <span class="comment">     * Recall that A_ij is mnp x cnp</span>
<a name="l01783"></a>01783 <span class="comment">     */</span>
<a name="l01784"></a>01784     <span class="comment">/* Also compute ea_j = \sum_i A_ij^T e_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l01785"></a>01785     <span class="comment">/* Recall that e_ij is mnp x 1</span>
<a name="l01786"></a>01786 <span class="comment">     */</span>
<a name="l01787"></a>01787           _dblzero(U, m*Usz); <span class="comment">/* clear all U_j */</span>
<a name="l01788"></a>01788           _dblzero(ea, m*easz); <span class="comment">/* clear all ea_j */</span>
<a name="l01789"></a>01789     <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01790"></a>01790       ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01791"></a>01791       ptr2=ea + j*easz; <span class="comment">// set ptr2 to point to ea_j</span>
<a name="l01792"></a>01792 
<a name="l01793"></a>01793       nnz=sba_crsm_col_elmidxs(&amp;idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero A_ij, i=0...n-1 */</span>
<a name="l01794"></a>01794       <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l01795"></a>01795         <span class="comment">/* set ptr3 to point to A_ij, actual row number in rcsubs[i] */</span>
<a name="l01796"></a>01796         ptr3=jac + idxij.val[rcidxs[i]]*Asz;
<a name="l01797"></a>01797 
<a name="l01798"></a>01798         <span class="comment">/* compute the UPPER TRIANGULAR PART of A_ij^T A_ij and add it to U_j */</span>
<a name="l01799"></a>01799         <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii){
<a name="l01800"></a>01800           <span class="keywordflow">for</span>(jj=ii; jj&lt;cnp; ++jj){
<a name="l01801"></a>01801             <span class="keywordflow">for</span>(k=0, sum=0.0; k&lt;mnp; ++k)
<a name="l01802"></a>01802               sum+=ptr3[k*cnp+ii]*ptr3[k*cnp+jj];
<a name="l01803"></a>01803             ptr1[ii*cnp+jj]+=sum;
<a name="l01804"></a>01804           }
<a name="l01805"></a>01805 
<a name="l01806"></a>01806           <span class="comment">/* copy the LOWER TRIANGULAR PART of U_j from the upper one */</span>
<a name="l01807"></a>01807           <span class="keywordflow">for</span>(jj=0; jj&lt;ii; ++jj)
<a name="l01808"></a>01808             ptr1[ii*cnp+jj]=ptr1[jj*cnp+ii];
<a name="l01809"></a>01809         }
<a name="l01810"></a>01810 
<a name="l01811"></a>01811         ptr4=e + idxij.val[rcidxs[i]]*esz; <span class="comment">/* set ptr4 to point to e_ij */</span>
<a name="l01812"></a>01812         <span class="comment">/* compute A_ij^T e_ij and add it to ea_j */</span>
<a name="l01813"></a>01813         <span class="keywordflow">for</span>(ii=0; ii&lt;cnp; ++ii){
<a name="l01814"></a>01814           <span class="keywordflow">for</span>(jj=0, sum=0.0; jj&lt;mnp; ++jj)
<a name="l01815"></a>01815             sum+=ptr3[jj*cnp+ii]*ptr4[jj];
<a name="l01816"></a>01816           ptr2[ii]+=sum;
<a name="l01817"></a>01817         }
<a name="l01818"></a>01818       }
<a name="l01819"></a>01819     }
<a name="l01820"></a>01820 
<a name="l01821"></a>01821     <span class="comment">/* Compute ||J^T e||_inf and ||p||^2 */</span>
<a name="l01822"></a>01822     <span class="keywordflow">for</span>(i=0, p_L2=ea_inf=0.0; i&lt;nvars; ++i){
<a name="l01823"></a>01823       <span class="keywordflow">if</span>(ea_inf &lt; (tmp=FABS(ea[i]))) ea_inf=tmp;
<a name="l01824"></a>01824       p_L2+=p[i]*p[i];
<a name="l01825"></a>01825     }
<a name="l01826"></a>01826     <span class="comment">//p_L2=sqrt(p_L2);</span>
<a name="l01827"></a>01827 
<a name="l01828"></a>01828     <span class="comment">/* save diagonal entries so that augmentation can be later canceled.</span>
<a name="l01829"></a>01829 <span class="comment">     * Diagonal entries are in U_j</span>
<a name="l01830"></a>01830 <span class="comment">     */</span>
<a name="l01831"></a>01831     <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01832"></a>01832       ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01833"></a>01833       ptr2=diagU + j*cnp; <span class="comment">// set ptr2 to point to diagU_j</span>
<a name="l01834"></a>01834       <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01835"></a>01835         ptr2[i]=ptr1[i*cnp+i];
<a name="l01836"></a>01836     }
<a name="l01837"></a>01837 
<a name="l01838"></a>01838 <span class="comment">/*</span>
<a name="l01839"></a>01839 <span class="comment">if(!(itno%100)){</span>
<a name="l01840"></a>01840 <span class="comment">  printf(&quot;Current estimate: &quot;);</span>
<a name="l01841"></a>01841 <span class="comment">  for(i=0; i&lt;nvars; ++i)</span>
<a name="l01842"></a>01842 <span class="comment">    printf(&quot;%.9g &quot;, p[i]);</span>
<a name="l01843"></a>01843 <span class="comment">  printf(&quot;-- errors %.9g %0.9g\n&quot;, ea_inf, p_eL2);</span>
<a name="l01844"></a>01844 <span class="comment">}</span>
<a name="l01845"></a>01845 <span class="comment">*/</span>
<a name="l01846"></a>01846 
<a name="l01847"></a>01847     <span class="comment">/* check for convergence */</span>
<a name="l01848"></a>01848     <span class="keywordflow">if</span>((ea_inf &lt;= eps1)){
<a name="l01849"></a>01849       dp_L2=0.0; <span class="comment">/* no increment for p in this case */</span>
<a name="l01850"></a>01850       stop=1;
<a name="l01851"></a>01851       <span class="keywordflow">break</span>;
<a name="l01852"></a>01852     }
<a name="l01853"></a>01853 
<a name="l01854"></a>01854    <span class="comment">/* compute initial damping factor */</span>
<a name="l01855"></a>01855     <span class="keywordflow">if</span>(itno==0){
<a name="l01856"></a>01856       <span class="keywordflow">for</span>(i=mcon*cnp, tmp=DBL_MIN; i&lt;nvars; ++i)
<a name="l01857"></a>01857         <span class="keywordflow">if</span>(diagU[i]&gt;tmp) tmp=diagU[i]; <span class="comment">/* find max diagonal element */</span>
<a name="l01858"></a>01858       mu=tau*tmp;
<a name="l01859"></a>01859     }
<a name="l01860"></a>01860 
<a name="l01861"></a>01861     <span class="comment">/* determine increment using adaptive damping */</span>
<a name="l01862"></a>01862     <span class="keywordflow">while</span>(1){
<a name="l01863"></a>01863       <span class="comment">/* augment U */</span>
<a name="l01864"></a>01864       <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01865"></a>01865         ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01866"></a>01866         <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01867"></a>01867           ptr1[i*cnp+i]+=mu;
<a name="l01868"></a>01868       }
<a name="l01869"></a>01869  
<a name="l01870"></a>01870       <span class="comment">/* solve the linear systems U_j da_j = ea_j to compute the da_j */</span>
<a name="l01871"></a>01871       _dblzero(dp, mcon*cnp); <span class="comment">/* no change for the first mcon camera params */</span>
<a name="l01872"></a>01872       <span class="keywordflow">for</span>(j=nsolved=mcon; j&lt;m; ++j){
<a name="l01873"></a>01873                     ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01874"></a>01874                     ptr2=ea + j*easz; <span class="comment">// set ptr2 to point to ea_j</span>
<a name="l01875"></a>01875                     ptr3=dp + j*cnp; <span class="comment">// set ptr3 to point to da_j</span>
<a name="l01876"></a>01876 
<a name="l01877"></a>01877                     <span class="comment">//nsolved+=sba_Axb_LU(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_LU;</span>
<a name="l01878"></a>01878                     nsolved+=sba_Axb_Chol(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_Chol;
<a name="l01879"></a>01879                     <span class="comment">//nsolved+=sba_Axb_BK(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_BK;</span>
<a name="l01880"></a>01880                     <span class="comment">//nsolved+=sba_Axb_QRnoQ(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_QRnoQ;</span>
<a name="l01881"></a>01881                     <span class="comment">//nsolved+=sba_Axb_QR(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_QR;</span>
<a name="l01882"></a>01882                     <span class="comment">//nsolved+=sba_Axb_SVD(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_SVD;</span>
<a name="l01883"></a>01883               <span class="comment">//nsolved+=(sba_Axb_CG(ptr1, ptr2, ptr3, cnp, (3*cnp)/2, 1E-10, SBA_CG_JACOBI, 0) &gt; 0); linsolver=(PLS)sba_Axb_CG;</span>
<a name="l01884"></a>01884 
<a name="l01885"></a>01885         ++nlss;
<a name="l01886"></a>01886             }
<a name="l01887"></a>01887 
<a name="l01888"></a>01888             <span class="keywordflow">if</span>(nsolved==m){
<a name="l01889"></a>01889 
<a name="l01890"></a>01890         <span class="comment">/* parameter vector updates are now in dp */</span>
<a name="l01891"></a>01891 
<a name="l01892"></a>01892         <span class="comment">/* compute p&#39;s new estimate and ||dp||^2 */</span>
<a name="l01893"></a>01893         <span class="keywordflow">for</span>(i=0, dp_L2=0.0; i&lt;nvars; ++i){
<a name="l01894"></a>01894           pdp[i]=p[i] + (tmp=dp[i]);
<a name="l01895"></a>01895           dp_L2+=tmp*tmp;
<a name="l01896"></a>01896         }
<a name="l01897"></a>01897         <span class="comment">//dp_L2=sqrt(dp_L2);</span>
<a name="l01898"></a>01898 
<a name="l01899"></a>01899         <span class="keywordflow">if</span>(dp_L2&lt;=eps2_sq*p_L2){ <span class="comment">/* relative change in p is small, stop */</span>
<a name="l01900"></a>01900         <span class="comment">//if(dp_L2&lt;=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */</span>
<a name="l01901"></a>01901           stop=2;
<a name="l01902"></a>01902           <span class="keywordflow">break</span>;
<a name="l01903"></a>01903         }
<a name="l01904"></a>01904 
<a name="l01905"></a>01905         <span class="keywordflow">if</span>(dp_L2&gt;=(p_L2+eps2)/SBA_EPSILON_SQ){ <span class="comment">/* almost singular */</span>
<a name="l01906"></a>01906         <span class="comment">//if(dp_L2&gt;=(p_L2+eps2)/SBA_EPSILON){ /* almost singular */</span>
<a name="l01907"></a>01907           fprintf(stderr, <span class="stringliteral">&quot;SBA: the matrix of the augmented normal equations is almost singular in sba_mot_levmar_x(),\n&quot;</span>
<a name="l01908"></a>01908                           <span class="stringliteral">&quot;     minimization should be restarted from the current solution with an increased damping term\n&quot;</span>);
<a name="l01909"></a>01909           retval=SBA_ERROR;
<a name="l01910"></a>01910           <span class="keywordflow">goto</span> freemem_and_return;
<a name="l01911"></a>01911         }
<a name="l01912"></a>01912 
<a name="l01913"></a>01913         (*func)(pdp, &amp;idxij, rcidxs, rcsubs, hx, adata); ++nfev; <span class="comment">/* evaluate function at p + dp */</span>
<a name="l01914"></a>01914         <span class="keywordflow">if</span>(verbose&gt;1)
<a name="l01915"></a>01915           printf(<span class="stringliteral">&quot;mean reprojection error %g\n&quot;</span>, sba_mean_repr_error(n, mnp, x, hx, &amp;idxij, rcidxs, rcsubs));
<a name="l01916"></a>01916         <span class="comment">/* ### compute ||e(pdp)||_2 */</span>
<a name="l01917"></a>01917         <span class="keywordflow">if</span>(covx==NULL)
<a name="l01918"></a>01918           pdp_eL2=nrmL2xmy(hx, x, hx, nobs); <span class="comment">/* hx=x-hx, pdp_eL2=||hx|| */</span>
<a name="l01919"></a>01919         <span class="keywordflow">else</span>
<a name="l01920"></a>01920           pdp_eL2=nrmCxmy(hx, x, hx, wght, mnp, nvis); <span class="comment">/* hx=wght*(x-hx), pdp_eL2=||hx|| */</span>
<a name="l01921"></a>01921         <span class="keywordflow">if</span>(!SBA_FINITE(pdp_eL2)){
<a name="l01922"></a>01922           <span class="keywordflow">if</span>(verbose) <span class="comment">/* identify the offending point projection */</span>
<a name="l01923"></a>01923             sba_print_inf(hx, m, mnp, &amp;idxij, rcidxs, rcsubs);
<a name="l01924"></a>01924 
<a name="l01925"></a>01925           stop=7;
<a name="l01926"></a>01926           <span class="keywordflow">break</span>;
<a name="l01927"></a>01927         }
<a name="l01928"></a>01928 
<a name="l01929"></a>01929         <span class="keywordflow">for</span>(i=0, dL=0.0; i&lt;nvars; ++i)
<a name="l01930"></a>01930           dL+=dp[i]*(mu*dp[i]+ea[i]);
<a name="l01931"></a>01931 
<a name="l01932"></a>01932         dF=p_eL2-pdp_eL2;
<a name="l01933"></a>01933 
<a name="l01934"></a>01934         <span class="keywordflow">if</span>(verbose&gt;1)
<a name="l01935"></a>01935           printf(<span class="stringliteral">&quot;\ndamping term %8g, gain ratio %8g, errors %8g / %8g = %g\n&quot;</span>, mu, dL!=0.0? dF/dL : dF/DBL_EPSILON, p_eL2/nvis, pdp_eL2/nvis, p_eL2/pdp_eL2);
<a name="l01936"></a>01936 
<a name="l01937"></a>01937         <span class="keywordflow">if</span>(dL&gt;0.0 &amp;&amp; dF&gt;0.0){ <span class="comment">/* reduction in error, increment is accepted */</span>
<a name="l01938"></a>01938           tmp=(2.0*dF/dL-1.0);
<a name="l01939"></a>01939           tmp=1.0-tmp*tmp*tmp;
<a name="l01940"></a>01940           mu=mu*( (tmp&gt;=SBA_ONE_THIRD)? tmp : SBA_ONE_THIRD );
<a name="l01941"></a>01941           nu=2;
<a name="l01942"></a>01942 
<a name="l01943"></a>01943           <span class="comment">/* the test below is equivalent to the relative reduction of the RMS reprojection error: sqrt(p_eL2)-sqrt(pdp_eL2)&lt;eps4*sqrt(p_eL2) */</span>
<a name="l01944"></a>01944           <span class="keywordflow">if</span>(pdp_eL2-2.0*sqrt(p_eL2*pdp_eL2)&lt;(eps4_sq-1.0)*p_eL2) stop=4;
<a name="l01945"></a>01945           
<a name="l01946"></a>01946           <span class="keywordflow">for</span>(i=0; i&lt;nvars; ++i) <span class="comment">/* update p&#39;s estimate */</span>
<a name="l01947"></a>01947             p[i]=pdp[i];
<a name="l01948"></a>01948 
<a name="l01949"></a>01949           <span class="keywordflow">for</span>(i=0; i&lt;nobs; ++i) <span class="comment">/* update e and ||e||_2 */</span>
<a name="l01950"></a>01950             e[i]=hx[i];
<a name="l01951"></a>01951           p_eL2=pdp_eL2;
<a name="l01952"></a>01952           <span class="keywordflow">break</span>;
<a name="l01953"></a>01953         }
<a name="l01954"></a>01954       } <span class="comment">/* nsolved==m */</span>
<a name="l01955"></a>01955 
<a name="l01956"></a>01956       <span class="comment">/* if this point is reached, either at least one linear system could not be solved or</span>
<a name="l01957"></a>01957 <span class="comment">       * the error did not reduce; in any case, the increment must be rejected</span>
<a name="l01958"></a>01958 <span class="comment">       */</span>
<a name="l01959"></a>01959 
<a name="l01960"></a>01960       mu*=nu;
<a name="l01961"></a>01961       nu2=nu&lt;&lt;1; <span class="comment">// 2*nu;</span>
<a name="l01962"></a>01962       <span class="keywordflow">if</span>(nu2&lt;=nu){ <span class="comment">/* nu has wrapped around (overflown) */</span>
<a name="l01963"></a>01963         fprintf(stderr, <span class="stringliteral">&quot;SBA: too many failed attempts to increase the damping factor in sba_mot_levmar_x()! Singular Hessian matrix?\n&quot;</span>);
<a name="l01964"></a>01964         <span class="comment">//exit(1);</span>
<a name="l01965"></a>01965         stop=6;
<a name="l01966"></a>01966         <span class="keywordflow">break</span>;
<a name="l01967"></a>01967       }
<a name="l01968"></a>01968       nu=nu2;
<a name="l01969"></a>01969 
<a name="l01970"></a>01970 <span class="preprocessor">#if 0</span>
<a name="l01971"></a>01971 <span class="preprocessor"></span>      <span class="comment">/* restore U diagonal entries */</span>
<a name="l01972"></a>01972       <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01973"></a>01973         ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01974"></a>01974         ptr2=diagU + j*cnp; <span class="comment">// set ptr2 to point to diagU_j</span>
<a name="l01975"></a>01975         <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01976"></a>01976           ptr1[i*cnp+i]=ptr2[i];
<a name="l01977"></a>01977       }
<a name="l01978"></a>01978 <span class="preprocessor">#endif</span>
<a name="l01979"></a>01979 <span class="preprocessor"></span>    } <span class="comment">/* inner while loop */</span>
<a name="l01980"></a>01980         <span class="comment">// Nota: fin bucle &#39;while&#39;.</span>
<a name="l01981"></a>01981 
<a name="l01982"></a>01982     <span class="keywordflow">if</span>(p_eL2&lt;=eps3_sq) stop=5; <span class="comment">// error is small, force termination of outer loop</span>
<a name="l01983"></a>01983   }
<a name="l01984"></a>01984 
<a name="l01985"></a>01985   <span class="keywordflow">if</span>(itno&gt;=itmax) stop=3;
<a name="l01986"></a>01986 
<a name="l01987"></a>01987   <span class="comment">/* restore U diagonal entries */</span>
<a name="l01988"></a>01988   <span class="keywordflow">for</span>(j=mcon; j&lt;m; ++j){
<a name="l01989"></a>01989     ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l01990"></a>01990     ptr2=diagU + j*cnp; <span class="comment">// set ptr2 to point to diagU_j</span>
<a name="l01991"></a>01991     <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l01992"></a>01992       ptr1[i*cnp+i]=ptr2[i];
<a name="l01993"></a>01993   }
<a name="l01994"></a>01994 
<a name="l01995"></a>01995   <span class="keywordflow">if</span>(info){
<a name="l01996"></a>01996     info[0]=init_p_eL2;
<a name="l01997"></a>01997     info[1]=p_eL2;
<a name="l01998"></a>01998     info[2]=ea_inf;
<a name="l01999"></a>01999     info[3]=dp_L2;
<a name="l02000"></a>02000     <span class="keywordflow">for</span>(j=mcon, tmp=DBL_MIN; j&lt;m; ++j){
<a name="l02001"></a>02001       ptr1=U + j*Usz; <span class="comment">// set ptr1 to point to U_j</span>
<a name="l02002"></a>02002       <span class="keywordflow">for</span>(i=0; i&lt;cnp; ++i)
<a name="l02003"></a>02003         <span class="keywordflow">if</span>(tmp&lt;ptr1[i*cnp+i]) tmp=ptr1[i*cnp+i];
<a name="l02004"></a>02004     }
<a name="l02005"></a>02005     info[4]=mu/tmp;
<a name="l02006"></a>02006     info[5]=itno;
<a name="l02007"></a>02007     info[6]=stop;
<a name="l02008"></a>02008     info[7]=nfev;
<a name="l02009"></a>02009     info[8]=njev;
<a name="l02010"></a>02010     info[9]=nlss;
<a name="l02011"></a>02011   }
<a name="l02012"></a>02012   <span class="comment">//sba_print_sol(n, m, p, cnp, 0, x, mnp, &amp;idxij, rcidxs, rcsubs);</span>
<a name="l02013"></a>02013   retval=(stop!=7)?  itno : SBA_ERROR;
<a name="l02014"></a>02014                                                                
<a name="l02015"></a>02015 freemem_and_return: <span class="comment">/* NOTE: this point is also reached via a goto! */</span>
<a name="l02016"></a>02016 
<a name="l02017"></a>02017    <span class="comment">/* free whatever was allocated */</span>
<a name="l02018"></a>02018   free(jac); free(U);
<a name="l02019"></a>02019   free(e); free(ea);  
<a name="l02020"></a>02020   free(dp);
<a name="l02021"></a>02021   free(rcidxs); free(rcsubs);
<a name="l02022"></a>02022 <span class="preprocessor">#ifndef SBA_DESTROY_COVS</span>
<a name="l02023"></a>02023 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(wght) free(wght);
<a name="l02024"></a>02024 <span class="preprocessor">#else</span>
<a name="l02025"></a>02025 <span class="preprocessor"></span>  <span class="comment">/* nothing to do */</span>
<a name="l02026"></a>02026 <span class="preprocessor">#endif </span><span class="comment">/* SBA_DESTROY_COVS */</span>
<a name="l02027"></a>02027 
<a name="l02028"></a>02028   free(hx); free(diagU); free(pdp);
<a name="l02029"></a>02029   <span class="keywordflow">if</span>(fdj_data.hxx){ <span class="comment">// cleanup</span>
<a name="l02030"></a>02030     free(fdj_data.hxx);
<a name="l02031"></a>02031     free(fdj_data.func_rcidxs);
<a name="l02032"></a>02032   }
<a name="l02033"></a>02033 
<a name="l02034"></a>02034   sba_crsm_free(&amp;idxij);
<a name="l02035"></a>02035 
<a name="l02036"></a>02036   <span class="comment">/* free the memory allocated by the linear solver routine */</span>
<a name="l02037"></a>02037   <span class="keywordflow">if</span>(linsolver) (*linsolver)(NULL, NULL, NULL, 0, 0);
<a name="l02038"></a>02038 
<a name="l02039"></a>02039   <span class="keywordflow">return</span> retval;
<a name="l02040"></a>02040 }
<a name="l02041"></a>02041 
<a name="l02042"></a>02042 
<a name="l02043"></a>02043 <span class="comment">/* Bundle adjustment on structure parameters only </span>
<a name="l02044"></a>02044 <span class="comment"> * using the sparse Levenberg-Marquardt as described in HZ p. 568</span>
<a name="l02045"></a>02045 <span class="comment"> *</span>
<a name="l02046"></a>02046 <span class="comment"> * Returns the number of iterations (&gt;=0) if successfull, SBA_ERROR if failed</span>
<a name="l02047"></a>02047 <span class="comment"> */</span>
<a name="l02048"></a>02048 
<a name="l02049"></a>02049 <span class="keywordtype">int</span> sba_str_levmar_x(
<a name="l02050"></a>02050     <span class="keyword">const</span> <span class="keywordtype">int</span> n,   <span class="comment">/* number of points */</span>
<a name="l02051"></a>02051     <span class="keyword">const</span> <span class="keywordtype">int</span> ncon,<span class="comment">/* number of points (starting from the 1st) whose parameters should not be modified.</span>
<a name="l02052"></a>02052 <span class="comment">                   * All B_ij (see below) with i&lt;ncon are assumed to be zero</span>
<a name="l02053"></a>02053 <span class="comment">                   */</span>
<a name="l02054"></a>02054     <span class="keyword">const</span> <span class="keywordtype">int</span> m,   <span class="comment">/* number of images */</span>
<a name="l02055"></a>02055     <span class="keywordtype">char</span> *vmask,  <span class="comment">/* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */</span>
<a name="l02056"></a>02056     <span class="keywordtype">double</span> *p,    <span class="comment">/* initial parameter vector p0: (b1, ..., bn).</span>
<a name="l02057"></a>02057 <span class="comment">                   * bi are the i-th point parameters, * size n*pnp */</span>
<a name="l02058"></a>02058     <span class="keyword">const</span> <span class="keywordtype">int</span> pnp,<span class="comment">/* number of parameters for ONE point; e.g. 3 for Euclidean points */</span>
<a name="l02059"></a>02059     <span class="keywordtype">double</span> *x,    <span class="comment">/* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where</span>
<a name="l02060"></a>02060 <span class="comment">                   * x_ij is the projection of the i-th point on the j-th image.</span>
<a name="l02061"></a>02061 <span class="comment">                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;</span>
<a name="l02062"></a>02062 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp</span>
<a name="l02063"></a>02063 <span class="comment">                   */</span>
<a name="l02064"></a>02064     <span class="keywordtype">double</span> *covx, <span class="comment">/* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm),</span>
<a name="l02065"></a>02065 <span class="comment">                   * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no</span>
<a name="l02066"></a>02066 <span class="comment">                   * covariance estimates are available (identity matrices are implicitly used in this case).</span>
<a name="l02067"></a>02067 <span class="comment">                   * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing;</span>
<a name="l02068"></a>02068 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp*mnp</span>
<a name="l02069"></a>02069 <span class="comment">                   */</span>
<a name="l02070"></a>02070     <span class="keyword">const</span> <span class="keywordtype">int</span> mnp,<span class="comment">/* number of parameters for EACH measurement; usually 2 */</span>
<a name="l02071"></a>02071     <span class="keywordtype">void</span> (*func)(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata),
<a name="l02072"></a>02072                                               <span class="comment">/* functional relation describing measurements. Given a parameter vector p,</span>
<a name="l02073"></a>02073 <span class="comment">                                               * computes a prediction of the measurements \hat{x}. p is (n*pnp)x1,</span>
<a name="l02074"></a>02074 <span class="comment">                                               * \hat{x} is (n*m*mnp)x1, maximum</span>
<a name="l02075"></a>02075 <span class="comment">                                               * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used</span>
<a name="l02076"></a>02076 <span class="comment">                                               * as working memory</span>
<a name="l02077"></a>02077 <span class="comment">                                               */</span>
<a name="l02078"></a>02078     <span class="keywordtype">void</span> (*fjac)(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata),
<a name="l02079"></a>02079                                               <span class="comment">/* function to evaluate the sparse jacobian dX/dp.</span>
<a name="l02080"></a>02080 <span class="comment">                                               * The Jacobian is returned in jac as</span>
<a name="l02081"></a>02081 <span class="comment">                                               * (dx_11/db_1, ..., dx_1m/db_1, ..., dx_n1/db_n, ..., dx_nm/db_n), or (using HZ&#39;s notation),</span>
<a name="l02082"></a>02082 <span class="comment">                                               * jac=(B_11, ..., B_1m, ..., B_n1, ..., B_nm)</span>
<a name="l02083"></a>02083 <span class="comment">                                               * Notice that depending on idxij, some of the B_ij might be missing.</span>
<a name="l02084"></a>02084 <span class="comment">                                               * Note also that B_ij are mnp x pnp matrices and they</span>
<a name="l02085"></a>02085 <span class="comment">                                               * should be stored in jac in row-major order.</span>
<a name="l02086"></a>02086 <span class="comment">                                               * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used</span>
<a name="l02087"></a>02087 <span class="comment">                                               * as working memory</span>
<a name="l02088"></a>02088 <span class="comment">                                               *</span>
<a name="l02089"></a>02089 <span class="comment">                                               * If NULL, the jacobian is approximated by repetitive func calls and finite</span>
<a name="l02090"></a>02090 <span class="comment">                                               * differences. This is computationally inefficient and thus NOT recommended.</span>
<a name="l02091"></a>02091 <span class="comment">                                               */</span>
<a name="l02092"></a>02092     <span class="keywordtype">void</span> *adata,       <span class="comment">/* pointer to possibly additional data, passed uninterpreted to func, fjac */</span> 
<a name="l02093"></a>02093 
<a name="l02094"></a>02094     <span class="keyword">const</span> <span class="keywordtype">int</span> itmax,   <span class="comment">/* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */</span>
<a name="l02095"></a>02095     <span class="keyword">const</span> <span class="keywordtype">int</span> verbose, <span class="comment">/* I: verbosity */</span>
<a name="l02096"></a>02096     <span class="keyword">const</span> <span class="keywordtype">double</span> opts[SBA_OPTSSZ],
<a name="l02097"></a>02097                              <span class="comment">/* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu,</span>
<a name="l02098"></a>02098 <span class="comment">                        * stopping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2</span>
<a name="l02099"></a>02099 <span class="comment">                        */</span>
<a name="l02100"></a>02100     <span class="keywordtype">double</span> info[SBA_INFOSZ]
<a name="l02101"></a>02101                              <span class="comment">/* O: information regarding the minimization. Set to NULL if don&#39;t care</span>
<a name="l02102"></a>02102 <span class="comment">                        * info[0]=||e||_2 at initial p.</span>
<a name="l02103"></a>02103 <span class="comment">                        * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.</span>
<a name="l02104"></a>02104 <span class="comment">                        * info[5]= # iterations,</span>
<a name="l02105"></a>02105 <span class="comment">                        * info[6]=reason for terminating: 1 - stopped by small gradient J^T e</span>
<a name="l02106"></a>02106 <span class="comment">                        *                                 2 - stopped by small dp</span>
<a name="l02107"></a>02107 <span class="comment">                        *                                 3 - stopped by itmax</span>
<a name="l02108"></a>02108 <span class="comment">                        *                                 4 - stopped by small relative reduction in ||e||_2</span>
<a name="l02109"></a>02109 <span class="comment">                        *                                 5 - stopped by small ||e||_2</span>
<a name="l02110"></a>02110 <span class="comment">                        *                                 6 - too many attempts to increase damping. Restart with increased mu</span>
<a name="l02111"></a>02111 <span class="comment">                        *                                 7 - stopped by invalid (i.e. NaN or Inf) &quot;func&quot; values. This is a user error</span>
<a name="l02112"></a>02112 <span class="comment">                        * info[7]= # function evaluations</span>
<a name="l02113"></a>02113 <span class="comment">                        * info[8]= # jacobian evaluations</span>
<a name="l02114"></a>02114 <span class="comment">                                          * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error</span>
<a name="l02115"></a>02115 <span class="comment">                        */</span>
<a name="l02116"></a>02116 )
<a name="l02117"></a>02117 {
<a name="l02118"></a>02118 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii, jj, k;
<a name="l02119"></a>02119 <span class="keywordtype">int</span> nvis, nnz, retval;
<a name="l02120"></a>02120 
<a name="l02121"></a>02121 <span class="comment">/* The following are work arrays that are dynamically allocated by sba_str_levmar_x() */</span>
<a name="l02122"></a>02122 <span class="keywordtype">double</span> *jac;  <span class="comment">/* work array for storing the jacobian, max. size n*m*mnp*pnp */</span>
<a name="l02123"></a>02123 <span class="keywordtype">double</span> *V;    <span class="comment">/* work array for storing the V_i in the order V_1, ..., V_n, size n*pnp*pnp */</span>
<a name="l02124"></a>02124 
<a name="l02125"></a>02125 <span class="keywordtype">double</span> *e;    <span class="comment">/* work array for storing the e_ij in the order e_11, ..., e_1m, ..., e_n1, ..., e_nm,</span>
<a name="l02126"></a>02126 <span class="comment">                 max. size n*m*mnp */</span>
<a name="l02127"></a>02127 <span class="keywordtype">double</span> *eb;   <span class="comment">/* work array for storing the eb_i in the order eb_1, .. eb_n size n*pnp */</span>
<a name="l02128"></a>02128 
<a name="l02129"></a>02129 <span class="keywordtype">double</span> *dp;   <span class="comment">/* work array for storing the parameter vector updates db_1, ..., db_n, size n*pnp */</span>
<a name="l02130"></a>02130 
<a name="l02131"></a>02131 <span class="keywordtype">double</span> *wght= <span class="comment">/* work array for storing the weights computed from the covariance inverses, max. size n*m*mnp*mnp */</span>
<a name="l02132"></a>02132             NULL;
<a name="l02133"></a>02133 
<a name="l02134"></a>02134 <span class="comment">/* Of the above arrays, jac, e, wght are sparse and</span>
<a name="l02135"></a>02135 <span class="comment"> * V, eb, dp are dense. Sparse arrays are indexed through</span>
<a name="l02136"></a>02136 <span class="comment"> * idxij (see below), that is with the same mechanism as the input </span>
<a name="l02137"></a>02137 <span class="comment"> * measurements vector x</span>
<a name="l02138"></a>02138 <span class="comment"> */</span>
<a name="l02139"></a>02139 
<a name="l02140"></a>02140 <span class="comment">/* submatrices sizes */</span>
<a name="l02141"></a>02141 <span class="keywordtype">int</span> Bsz, Vsz,
<a name="l02142"></a>02142     esz, ebsz, covsz;
<a name="l02143"></a>02143 
<a name="l02144"></a>02144 <span class="keyword">register</span> <span class="keywordtype">double</span> *ptr1, *ptr2, *ptr3, *ptr4, sum;
<a name="l02145"></a>02145 <span class="keyword">struct </span>sba_crsm idxij; <span class="comment">/* sparse matrix containing the location of x_ij in x. This is also the location</span>
<a name="l02146"></a>02146 <span class="comment">                        * of B_ij in jac, etc.</span>
<a name="l02147"></a>02147 <span class="comment">                        * This matrix can be thought as a map from a sparse set of pairs (i, j) to a continuous</span>
<a name="l02148"></a>02148 <span class="comment">                        * index k and it is used to efficiently lookup the memory locations where the non-zero</span>
<a name="l02149"></a>02149 <span class="comment">                        * blocks of a sparse matrix/vector are stored</span>
<a name="l02150"></a>02150 <span class="comment">                        */</span>
<a name="l02151"></a>02151 <span class="keywordtype">int</span> maxCPvis, <span class="comment">/* max. of projections across cameras &amp; projections across points */</span>
<a name="l02152"></a>02152     *rcidxs,  <span class="comment">/* work array for the indexes corresponding to the nonzero elements of a single row or</span>
<a name="l02153"></a>02153 <span class="comment">                 column in a sparse matrix, size max(n, m) */</span>
<a name="l02154"></a>02154     *rcsubs;  <span class="comment">/* work array for the subscripts of nonzero elements in a single row or column of a</span>
<a name="l02155"></a>02155 <span class="comment">                 sparse matrix, size max(n, m) */</span>
<a name="l02156"></a>02156 
<a name="l02157"></a>02157 <span class="comment">/* The following variables are needed by the LM algorithm */</span>
<a name="l02158"></a>02158 <span class="keyword">register</span> <span class="keywordtype">int</span> itno;  <span class="comment">/* iteration counter */</span>
<a name="l02159"></a>02159 <span class="keywordtype">int</span> nsolved;
<a name="l02160"></a>02160 <span class="comment">/* temporary work arrays that are dynamically allocated */</span>
<a name="l02161"></a>02161 <span class="keywordtype">double</span> *hx,         <span class="comment">/* \hat{x}_i, max. size m*n*mnp */</span>
<a name="l02162"></a>02162        *diagV,      <span class="comment">/* diagonals of V_i, size n*pnp */</span>
<a name="l02163"></a>02163        *pdp;        <span class="comment">/* p + dp, size n*pnp */</span>
<a name="l02164"></a>02164 
<a name="l02165"></a>02165 <span class="keyword">register</span> <span class="keywordtype">double</span> mu,  <span class="comment">/* damping constant */</span>
<a name="l02166"></a>02166                 tmp; <span class="comment">/* mainly used in matrix &amp; vector multiplications */</span>
<a name="l02167"></a>02167 <span class="keywordtype">double</span> p_eL2, eb_inf, pdp_eL2; <span class="comment">/* ||e(p)||_2, ||J^T e||_inf, ||e(p+dp)||_2 */</span>
<a name="l02168"></a>02168 <span class="keywordtype">double</span> p_L2, dp_L2=DBL_MAX, dF, dL;
<a name="l02169"></a>02169 <span class="keywordtype">double</span> tau=FABS(opts[0]), eps1=FABS(opts[1]), eps2=FABS(opts[2]), eps2_sq=opts[2]*opts[2],
<a name="l02170"></a>02170        eps3_sq=opts[3]*opts[3], eps4_sq=opts[4]*opts[4];
<a name="l02171"></a>02171 <span class="keywordtype">double</span> init_p_eL2;
<a name="l02172"></a>02172 <span class="keywordtype">int</span> nu=2, nu2, stop=0, nfev, njev=0, nlss=0;
<a name="l02173"></a>02173 <span class="keywordtype">int</span> nobs, nvars;
<a name="l02174"></a>02174 PLS linsolver=NULL;
<a name="l02175"></a>02175 
<a name="l02176"></a>02176 <span class="keyword">struct </span>fdj_data_x_ fdj_data;
<a name="l02177"></a>02177 <span class="keywordtype">void</span> *jac_adata;
<a name="l02178"></a>02178 
<a name="l02179"></a>02179 <span class="comment">/* Initialization */</span>
<a name="l02180"></a>02180   mu=eb_inf=tmp=0.0; <span class="comment">/* -Wall */</span>
<a name="l02181"></a>02181 
<a name="l02182"></a>02182   <span class="comment">/* block sizes */</span>
<a name="l02183"></a>02183   Bsz=mnp * pnp; Vsz=pnp * pnp;
<a name="l02184"></a>02184   esz=mnp; ebsz=pnp;
<a name="l02185"></a>02185   covsz=mnp * mnp;
<a name="l02186"></a>02186 
<a name="l02187"></a>02187   <span class="comment">/* count total number of visible image points */</span>
<a name="l02188"></a>02188   <span class="keywordflow">for</span>(i=nvis=0, jj=n*m; i&lt;jj; ++i)
<a name="l02189"></a>02189     nvis+=(vmask[i]!=0);
<a name="l02190"></a>02190 
<a name="l02191"></a>02191   nobs=nvis*mnp;
<a name="l02192"></a>02192   nvars=n*pnp;
<a name="l02193"></a>02193   <span class="keywordflow">if</span>(nobs&lt;nvars){
<a name="l02194"></a>02194     fprintf(stderr, <span class="stringliteral">&quot;SBA: sba_str_levmar_x() cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n&quot;</span>, nobs, nvars);
<a name="l02195"></a>02195     <span class="keywordflow">return</span> SBA_ERROR;
<a name="l02196"></a>02196   }
<a name="l02197"></a>02197 
<a name="l02198"></a>02198   <span class="comment">/* allocate &amp; fill up the idxij structure */</span>
<a name="l02199"></a>02199   sba_crsm_alloc(&amp;idxij, n, m, nvis);
<a name="l02200"></a>02200   <span class="keywordflow">for</span>(i=k=0; i&lt;n; ++i){
<a name="l02201"></a>02201     idxij.rowptr[i]=k;
<a name="l02202"></a>02202     ii=i*m;
<a name="l02203"></a>02203     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l02204"></a>02204       <span class="keywordflow">if</span>(vmask[ii+j]){
<a name="l02205"></a>02205         idxij.val[k]=k;
<a name="l02206"></a>02206         idxij.colidx[k++]=j;
<a name="l02207"></a>02207       }
<a name="l02208"></a>02208   }
<a name="l02209"></a>02209   idxij.rowptr[n]=nvis;
<a name="l02210"></a>02210 
<a name="l02211"></a>02211   <span class="comment">/* find the maximum number of visible image points in any single camera or coming from a single 3D point */</span>
<a name="l02212"></a>02212   <span class="comment">/* cameras */</span>
<a name="l02213"></a>02213   <span class="keywordflow">for</span>(i=maxCPvis=0; i&lt;n; ++i)
<a name="l02214"></a>02214     <span class="keywordflow">if</span>((k=idxij.rowptr[i+1]-idxij.rowptr[i])&gt;maxCPvis) maxCPvis=k;
<a name="l02215"></a>02215 
<a name="l02216"></a>02216   <span class="comment">/* points, note that maxCPvis is not reinitialized! */</span>
<a name="l02217"></a>02217   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l02218"></a>02218     <span class="keywordflow">for</span>(i=ii=0; i&lt;n; ++i)
<a name="l02219"></a>02219       <span class="keywordflow">if</span>(vmask[i*m+j]) ++ii;
<a name="l02220"></a>02220     <span class="keywordflow">if</span>(ii&gt;maxCPvis) maxCPvis=ii;
<a name="l02221"></a>02221   }
<a name="l02222"></a>02222 
<a name="l02223"></a>02223   <span class="comment">/* allocate work arrays */</span>
<a name="l02224"></a>02224   jac=(<span class="keywordtype">double</span> *)emalloc(nvis*Bsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02225"></a>02225   V=(<span class="keywordtype">double</span> *)emalloc(n*Vsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02226"></a>02226   e=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02227"></a>02227   eb=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02228"></a>02228   dp=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02229"></a>02229   rcidxs=(<span class="keywordtype">int</span> *)emalloc(maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l02230"></a>02230   rcsubs=(<span class="keywordtype">int</span> *)emalloc(maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l02231"></a>02231 <span class="preprocessor">#ifndef SBA_DESTROY_COVS</span>
<a name="l02232"></a>02232 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(covx!=NULL) wght=(<span class="keywordtype">double</span> *)emalloc(nvis*covsz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02233"></a>02233 <span class="preprocessor">#else</span>
<a name="l02234"></a>02234 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(covx!=NULL) wght=covx;
<a name="l02235"></a>02235 <span class="preprocessor">#endif </span><span class="comment">/* SBA_DESTROY_COVS */</span>
<a name="l02236"></a>02236 
<a name="l02237"></a>02237 
<a name="l02238"></a>02238   hx=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02239"></a>02239   diagV=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02240"></a>02240   pdp=(<span class="keywordtype">double</span> *)emalloc(nvars*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02241"></a>02241 
<a name="l02242"></a>02242   <span class="comment">/* if no jacobian function is supplied, prepare to compute jacobian with finite difference */</span>
<a name="l02243"></a>02243   <span class="keywordflow">if</span>(!fjac){
<a name="l02244"></a>02244     fdj_data.func=func;
<a name="l02245"></a>02245     fdj_data.cnp=0;
<a name="l02246"></a>02246     fdj_data.pnp=pnp;
<a name="l02247"></a>02247     fdj_data.mnp=mnp;
<a name="l02248"></a>02248     fdj_data.hx=hx;
<a name="l02249"></a>02249     fdj_data.hxx=(<span class="keywordtype">double</span> *)emalloc(nobs*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l02250"></a>02250     fdj_data.func_rcidxs=(<span class="keywordtype">int</span> *)emalloc(2*maxCPvis*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>));
<a name="l02251"></a>02251     fdj_data.func_rcsubs=fdj_data.func_rcidxs+maxCPvis;
<a name="l02252"></a>02252     fdj_data.adata=adata;
<a name="l02253"></a>02253 
<a name="l02254"></a>02254     fjac=sba_fdjac_x;
<a name="l02255"></a>02255     jac_adata=(<span class="keywordtype">void</span> *)&amp;fdj_data;
<a name="l02256"></a>02256   }
<a name="l02257"></a>02257   <span class="keywordflow">else</span>{
<a name="l02258"></a>02258     fdj_data.hxx=NULL;
<a name="l02259"></a>02259     jac_adata=adata;
<a name="l02260"></a>02260   }
<a name="l02261"></a>02261 
<a name="l02262"></a>02262   <span class="keywordflow">if</span>(itmax==0){ <span class="comment">/* verify jacobian */</span>
<a name="l02263"></a>02263     sba_str_chkjac_x(func, fjac, p, &amp;idxij, rcidxs, rcsubs, ncon, pnp, mnp, adata, jac_adata);
<a name="l02264"></a>02264     retval=0;
<a name="l02265"></a>02265     <span class="keywordflow">goto</span> freemem_and_return;
<a name="l02266"></a>02266   }
<a name="l02267"></a>02267 
<a name="l02268"></a>02268   <span class="comment">/* covariances Sigma_x_ij are accommodated by computing the Cholesky decompositions of their</span>
<a name="l02269"></a>02269 <span class="comment">   * inverses and using the resulting matrices w_x_ij to weigh B_ij and e_ij as</span>
<a name="l02270"></a>02270 <span class="comment">   * w_x_ij*B_ij and w_x_ij*e_ij. In this way, auxiliary variables as V_i=\sum_j B_ij^T B_ij</span>
<a name="l02271"></a>02271 <span class="comment">   * actually become \sum_j (w_x_ij B_ij)^T w_x_ij B_ij= \sum_j B_ij^T w_x_ij^T w_x_ij B_ij =</span>
<a name="l02272"></a>02272 <span class="comment">   * B_ij^T Sigma_x_ij^-1 B_ij</span>
<a name="l02273"></a>02273 <span class="comment">   *</span>
<a name="l02274"></a>02274 <span class="comment">   * eb_i are weighted in a similar manner</span>
<a name="l02275"></a>02275 <span class="comment">   */</span>
<a name="l02276"></a>02276   <span class="keywordflow">if</span>(covx!=NULL){
<a name="l02277"></a>02277     <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l02278"></a>02278       nnz=sba_crsm_row_elmidxs(&amp;idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero x_ij, j=0...m-1 */</span>
<a name="l02279"></a>02279       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l02280"></a>02280         <span class="comment">/* set ptr1, ptr2 to point to cov_x_ij, w_x_ij resp. */</span>
<a name="l02281"></a>02281         ptr1=covx + (k=idxij.val[rcidxs[j]]*covsz);
<a name="l02282"></a>02282         ptr2=wght + k;
<a name="l02283"></a>02283         <span class="keywordflow">if</span>(!sba_mat_cholinv(ptr1, ptr2, mnp)){ <span class="comment">/* compute w_x_ij s.t. w_x_ij^T w_x_ij = cov_x_ij^-1 */</span>
<a name="l02284"></a>02284                             fprintf(stderr, <span class="stringliteral">&quot;SBA: invalid covariance matrix for x_ij (i=%d, j=%d) in sba_motstr_levmar_x()\n&quot;</span>, i, rcsubs[j]);
<a name="l02285"></a>02285           retval=SBA_ERROR;
<a name="l02286"></a>02286           <span class="keywordflow">goto</span> freemem_and_return;
<a name="l02287"></a>02287         }
<a name="l02288"></a>02288       }
<a name="l02289"></a>02289     }
<a name="l02290"></a>02290     sba_mat_cholinv(NULL, NULL, 0); <span class="comment">/* cleanup */</span>
<a name="l02291"></a>02291   }
<a name="l02292"></a>02292 
<a name="l02293"></a>02293   <span class="comment">/* compute the error vectors e_ij in hx */</span>
<a name="l02294"></a>02294   (*func)(p, &amp;idxij, rcidxs, rcsubs, hx, adata); nfev=1;
<a name="l02295"></a>02295   <span class="comment">/* ### compute e=x - f(p) [e=w*(x - f(p)] and its L2 norm */</span>
<a name="l02296"></a>02296   <span class="keywordflow">if</span>(covx==NULL)
<a name="l02297"></a>02297     p_eL2=nrmL2xmy(e, x, hx, nobs); <span class="comment">/* e=x-hx, p_eL2=||e|| */</span>
<a name="l02298"></a>02298   <span class="keywordflow">else</span>
<a name="l02299"></a>02299     p_eL2=nrmCxmy(e, x, hx, wght, mnp, nvis); <span class="comment">/* e=wght*(x-hx), p_eL2=||e||=||x-hx||_Sigma^-1 */</span>
<a name="l02300"></a>02300   <span class="keywordflow">if</span>(verbose) printf(<span class="stringliteral">&quot;initial str-SBA error %g [%g]\n&quot;</span>, p_eL2, p_eL2/nvis);
<a name="l02301"></a>02301   init_p_eL2=p_eL2;
<a name="l02302"></a>02302   <span class="keywordflow">if</span>(!SBA_FINITE(p_eL2)) stop=7;
<a name="l02303"></a>02303 
<a name="l02304"></a>02304   <span class="keywordflow">for</span>(itno=0; itno&lt;itmax &amp;&amp; !stop; ++itno){
<a name="l02305"></a>02305     <span class="comment">/* Note that p, e and ||e||_2 have been updated at the previous iteration */</span>
<a name="l02306"></a>02306 
<a name="l02307"></a>02307     <span class="comment">/* compute derivative submatrices B_ij */</span>
<a name="l02308"></a>02308     (*fjac)(p, &amp;idxij, rcidxs, rcsubs, jac, jac_adata); ++njev;
<a name="l02309"></a>02309 
<a name="l02310"></a>02310     <span class="keywordflow">if</span>(covx!=NULL){
<a name="l02311"></a>02311       <span class="comment">/* compute w_x_ij B_ij.</span>
<a name="l02312"></a>02312 <span class="comment">       * Since w_x_ij is upper triangular, the products can be safely saved</span>
<a name="l02313"></a>02313 <span class="comment">       * directly in B_ij, without the need for intermediate storage</span>
<a name="l02314"></a>02314 <span class="comment">       */</span>
<a name="l02315"></a>02315       <span class="keywordflow">for</span>(i=0; i&lt;nvis; ++i){
<a name="l02316"></a>02316         <span class="comment">/* set ptr1, ptr2 to point to w_x_ij, B_ij, resp. */</span>
<a name="l02317"></a>02317         ptr1=wght + i*covsz;
<a name="l02318"></a>02318         ptr2=jac  + i*Bsz;
<a name="l02319"></a>02319 
<a name="l02320"></a>02320         <span class="comment">/* w_x_ij is mnp x mnp, B_ij is mnp x pnp */</span>
<a name="l02321"></a>02321         <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l02322"></a>02322           <span class="keywordflow">for</span>(jj=0; jj&lt;pnp; ++jj){
<a name="l02323"></a>02323             <span class="keywordflow">for</span>(k=ii, sum=0.0; k&lt;mnp; ++k) <span class="comment">// k&gt;=ii since w_x_ij is upper triangular</span>
<a name="l02324"></a>02324               sum+=ptr1[ii*mnp+k]*ptr2[k*pnp+jj];
<a name="l02325"></a>02325             ptr2[ii*pnp+jj]=sum;
<a name="l02326"></a>02326           }
<a name="l02327"></a>02327       }
<a name="l02328"></a>02328     }
<a name="l02329"></a>02329 
<a name="l02330"></a>02330     <span class="comment">/* compute V_i = \sum_j B_ij^T B_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l02331"></a>02331     <span class="comment">/* V_i is symmetric, therefore its computation can be sped up by</span>
<a name="l02332"></a>02332 <span class="comment">     * computing only the upper part and then reusing it for the lower one.</span>
<a name="l02333"></a>02333 <span class="comment">     * Recall that B_ij is mnp x pnp</span>
<a name="l02334"></a>02334 <span class="comment">     */</span>
<a name="l02335"></a>02335     <span class="comment">/* Also compute eb_i = \sum_j B_ij^T e_ij */</span> <span class="comment">// \Sigma here!</span>
<a name="l02336"></a>02336     <span class="comment">/* Recall that e_ij is mnp x 1</span>
<a name="l02337"></a>02337 <span class="comment">     */</span>
<a name="l02338"></a>02338           _dblzero(V, n*Vsz); <span class="comment">/* clear all V_i */</span>
<a name="l02339"></a>02339           _dblzero(eb, n*ebsz); <span class="comment">/* clear all eb_i */</span>
<a name="l02340"></a>02340     <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l02341"></a>02341       ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l02342"></a>02342       ptr2=eb + i*ebsz; <span class="comment">// set ptr2 to point to eb_i</span>
<a name="l02343"></a>02343 
<a name="l02344"></a>02344       nnz=sba_crsm_row_elmidxs(&amp;idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero B_ij, j=0...m-1 */</span>
<a name="l02345"></a>02345       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l02346"></a>02346         <span class="comment">/* set ptr3 to point to B_ij, actual column number in rcsubs[j] */</span>
<a name="l02347"></a>02347         ptr3=jac + idxij.val[rcidxs[j]]*Bsz;
<a name="l02348"></a>02348       
<a name="l02349"></a>02349         <span class="comment">/* compute the UPPER TRIANGULAR PART of B_ij^T B_ij and add it to V_i */</span>
<a name="l02350"></a>02350         <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii){
<a name="l02351"></a>02351           <span class="keywordflow">for</span>(jj=ii; jj&lt;pnp; ++jj){
<a name="l02352"></a>02352             <span class="keywordflow">for</span>(k=0, sum=0.0; k&lt;mnp; ++k)
<a name="l02353"></a>02353               sum+=ptr3[k*pnp+ii]*ptr3[k*pnp+jj];
<a name="l02354"></a>02354             ptr1[ii*pnp+jj]+=sum;
<a name="l02355"></a>02355           }
<a name="l02356"></a>02356 
<a name="l02357"></a>02357           <span class="comment">/* copy the LOWER TRIANGULAR PART of V_i from the upper one */</span>
<a name="l02358"></a>02358           <span class="keywordflow">for</span>(jj=0; jj&lt;ii; ++jj)
<a name="l02359"></a>02359             ptr1[ii*pnp+jj]=ptr1[jj*pnp+ii];
<a name="l02360"></a>02360         }
<a name="l02361"></a>02361 
<a name="l02362"></a>02362         ptr4=e + idxij.val[rcidxs[j]]*esz; <span class="comment">/* set ptr4 to point to e_ij */</span>
<a name="l02363"></a>02363         <span class="comment">/* compute B_ij^T e_ij and add it to eb_i */</span>
<a name="l02364"></a>02364         <span class="keywordflow">for</span>(ii=0; ii&lt;pnp; ++ii){
<a name="l02365"></a>02365           <span class="keywordflow">for</span>(jj=0, sum=0.0; jj&lt;mnp; ++jj)
<a name="l02366"></a>02366             sum+=ptr3[jj*pnp+ii]*ptr4[jj];
<a name="l02367"></a>02367           ptr2[ii]+=sum;
<a name="l02368"></a>02368         }
<a name="l02369"></a>02369       }
<a name="l02370"></a>02370     }
<a name="l02371"></a>02371 
<a name="l02372"></a>02372     <span class="comment">/* Compute ||J^T e||_inf and ||p||^2 */</span>
<a name="l02373"></a>02373     <span class="keywordflow">for</span>(i=0, p_L2=eb_inf=0.0; i&lt;nvars; ++i){
<a name="l02374"></a>02374       <span class="keywordflow">if</span>(eb_inf &lt; (tmp=FABS(eb[i]))) eb_inf=tmp;
<a name="l02375"></a>02375       p_L2+=p[i]*p[i];
<a name="l02376"></a>02376     }
<a name="l02377"></a>02377     <span class="comment">//p_L2=sqrt(p_L2);</span>
<a name="l02378"></a>02378 
<a name="l02379"></a>02379     <span class="comment">/* save diagonal entries so that augmentation can be later canceled.</span>
<a name="l02380"></a>02380 <span class="comment">     * Diagonal entries are in V_i</span>
<a name="l02381"></a>02381 <span class="comment">     */</span>
<a name="l02382"></a>02382     <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l02383"></a>02383       ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l02384"></a>02384       ptr2=diagV + i*pnp; <span class="comment">// set ptr2 to point to diagV_i</span>
<a name="l02385"></a>02385       <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l02386"></a>02386         ptr2[j]=ptr1[j*pnp+j];
<a name="l02387"></a>02387     }
<a name="l02388"></a>02388 
<a name="l02389"></a>02389 <span class="comment">/*</span>
<a name="l02390"></a>02390 <span class="comment">if(!(itno%100)){</span>
<a name="l02391"></a>02391 <span class="comment">  printf(&quot;Current estimate: &quot;);</span>
<a name="l02392"></a>02392 <span class="comment">  for(i=0; i&lt;nvars; ++i)</span>
<a name="l02393"></a>02393 <span class="comment">    printf(&quot;%.9g &quot;, p[i]);</span>
<a name="l02394"></a>02394 <span class="comment">  printf(&quot;-- errors %.9g %0.9g\n&quot;, eb_inf, p_eL2);</span>
<a name="l02395"></a>02395 <span class="comment">}</span>
<a name="l02396"></a>02396 <span class="comment">*/</span>
<a name="l02397"></a>02397 
<a name="l02398"></a>02398     <span class="comment">/* check for convergence */</span>
<a name="l02399"></a>02399     <span class="keywordflow">if</span>((eb_inf &lt;= eps1)){
<a name="l02400"></a>02400       dp_L2=0.0; <span class="comment">/* no increment for p in this case */</span>
<a name="l02401"></a>02401       stop=1;
<a name="l02402"></a>02402       <span class="keywordflow">break</span>;
<a name="l02403"></a>02403     }
<a name="l02404"></a>02404 
<a name="l02405"></a>02405    <span class="comment">/* compute initial damping factor */</span>
<a name="l02406"></a>02406     <span class="keywordflow">if</span>(itno==0){
<a name="l02407"></a>02407       <span class="keywordflow">for</span>(i=ncon*pnp, tmp=DBL_MIN; i&lt;nvars; ++i)
<a name="l02408"></a>02408         <span class="keywordflow">if</span>(diagV[i]&gt;tmp) tmp=diagV[i]; <span class="comment">/* find max diagonal element */</span>
<a name="l02409"></a>02409       mu=tau*tmp;
<a name="l02410"></a>02410     }
<a name="l02411"></a>02411 
<a name="l02412"></a>02412     <span class="comment">/* determine increment using adaptive damping */</span>
<a name="l02413"></a>02413     <span class="keywordflow">while</span>(1){
<a name="l02414"></a>02414       <span class="comment">/* augment V */</span>
<a name="l02415"></a>02415       <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l02416"></a>02416         ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l02417"></a>02417         <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l02418"></a>02418           ptr1[j*pnp+j]+=mu;
<a name="l02419"></a>02419       }
<a name="l02420"></a>02420 
<a name="l02421"></a>02421       <span class="comment">/* solve the linear systems V*_i db_i = eb_i to compute the db_i */</span>
<a name="l02422"></a>02422       _dblzero(dp, ncon*pnp); <span class="comment">/* no change for the first ncon point params */</span>
<a name="l02423"></a>02423       <span class="keywordflow">for</span>(i=nsolved=ncon; i&lt;n; ++i){
<a name="l02424"></a>02424         ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l02425"></a>02425         ptr2=eb + i*ebsz; <span class="comment">// set ptr2 to point to eb_i</span>
<a name="l02426"></a>02426         ptr3=dp + i*pnp; <span class="comment">// set ptr3 to point to db_i</span>
<a name="l02427"></a>02427 
<a name="l02428"></a>02428         <span class="comment">//nsolved+=sba_Axb_LU(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_LU;</span>
<a name="l02429"></a>02429         nsolved+=sba_Axb_Chol(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_Chol;
<a name="l02430"></a>02430         <span class="comment">//nsolved+=sba_Axb_BK(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_BK;</span>
<a name="l02431"></a>02431         <span class="comment">//nsolved+=sba_Axb_QRnoQ(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_QRnoQ;</span>
<a name="l02432"></a>02432         <span class="comment">//nsolved+=sba_Axb_QR(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_QR;</span>
<a name="l02433"></a>02433         <span class="comment">//nsolved+=sba_Axb_SVD(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_SVD;</span>
<a name="l02434"></a>02434               <span class="comment">//nsolved+=(sba_Axb_CG(ptr1, ptr2, ptr3, pnp, (3*pnp)/2, 1E-10, SBA_CG_JACOBI, 0) &gt; 0); linsolver=(PLS)sba_Axb_CG;</span>
<a name="l02435"></a>02435 
<a name="l02436"></a>02436         ++nlss;
<a name="l02437"></a>02437       }
<a name="l02438"></a>02438 
<a name="l02439"></a>02439       <span class="keywordflow">if</span>(nsolved==n){
<a name="l02440"></a>02440 
<a name="l02441"></a>02441         <span class="comment">/* parameter vector updates are now in dp */</span>
<a name="l02442"></a>02442 
<a name="l02443"></a>02443         <span class="comment">/* compute p&#39;s new estimate and ||dp||^2 */</span>
<a name="l02444"></a>02444         <span class="keywordflow">for</span>(i=0, dp_L2=0.0; i&lt;nvars; ++i){
<a name="l02445"></a>02445           pdp[i]=p[i] + (tmp=dp[i]);
<a name="l02446"></a>02446           dp_L2+=tmp*tmp;
<a name="l02447"></a>02447         }
<a name="l02448"></a>02448         <span class="comment">//dp_L2=sqrt(dp_L2);</span>
<a name="l02449"></a>02449 
<a name="l02450"></a>02450         <span class="keywordflow">if</span>(dp_L2&lt;=eps2_sq*p_L2){ <span class="comment">/* relative change in p is small, stop */</span>
<a name="l02451"></a>02451         <span class="comment">//if(dp_L2&lt;=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */</span>
<a name="l02452"></a>02452           stop=2;
<a name="l02453"></a>02453           <span class="keywordflow">break</span>;
<a name="l02454"></a>02454         }
<a name="l02455"></a>02455 
<a name="l02456"></a>02456         <span class="keywordflow">if</span>(dp_L2&gt;=(p_L2+eps2)/SBA_EPSILON_SQ){ <span class="comment">/* almost singular */</span>
<a name="l02457"></a>02457         <span class="comment">//if(dp_L2&gt;=(p_L2+eps2)/SBA_EPSILON){ /* almost singular */</span>
<a name="l02458"></a>02458           fprintf(stderr, <span class="stringliteral">&quot;SBA: the matrix of the augmented normal equations is almost singular in sba_motstr_levmar_x(),\n&quot;</span>
<a name="l02459"></a>02459                           <span class="stringliteral">&quot;     minimization should be restarted from the current solution with an increased damping term\n&quot;</span>);
<a name="l02460"></a>02460           retval=SBA_ERROR;
<a name="l02461"></a>02461           <span class="keywordflow">goto</span> freemem_and_return;
<a name="l02462"></a>02462         }
<a name="l02463"></a>02463 
<a name="l02464"></a>02464         (*func)(pdp, &amp;idxij, rcidxs, rcsubs, hx, adata); ++nfev; <span class="comment">/* evaluate function at p + dp */</span>
<a name="l02465"></a>02465         <span class="keywordflow">if</span>(verbose&gt;1)
<a name="l02466"></a>02466           printf(<span class="stringliteral">&quot;mean reprojection error %g\n&quot;</span>, sba_mean_repr_error(n, mnp, x, hx, &amp;idxij, rcidxs, rcsubs));
<a name="l02467"></a>02467         <span class="comment">/* ### compute ||e(pdp)||_2 */</span>
<a name="l02468"></a>02468         <span class="keywordflow">if</span>(covx==NULL)
<a name="l02469"></a>02469           pdp_eL2=nrmL2xmy(hx, x, hx, nobs); <span class="comment">/* hx=x-hx, pdp_eL2=||hx|| */</span>
<a name="l02470"></a>02470         <span class="keywordflow">else</span>
<a name="l02471"></a>02471           pdp_eL2=nrmCxmy(hx, x, hx, wght, mnp, nvis); <span class="comment">/* hx=wght*(x-hx), pdp_eL2=||hx|| */</span>
<a name="l02472"></a>02472         <span class="keywordflow">if</span>(!SBA_FINITE(pdp_eL2)){
<a name="l02473"></a>02473           <span class="keywordflow">if</span>(verbose) <span class="comment">/* identify the offending point projection */</span>
<a name="l02474"></a>02474             sba_print_inf(hx, m, mnp, &amp;idxij, rcidxs, rcsubs);
<a name="l02475"></a>02475 
<a name="l02476"></a>02476           stop=7;
<a name="l02477"></a>02477           <span class="keywordflow">break</span>;
<a name="l02478"></a>02478         }
<a name="l02479"></a>02479 
<a name="l02480"></a>02480         <span class="keywordflow">for</span>(i=0, dL=0.0; i&lt;nvars; ++i)
<a name="l02481"></a>02481           dL+=dp[i]*(mu*dp[i]+eb[i]);
<a name="l02482"></a>02482 
<a name="l02483"></a>02483         dF=p_eL2-pdp_eL2;
<a name="l02484"></a>02484 
<a name="l02485"></a>02485         <span class="keywordflow">if</span>(verbose&gt;1)
<a name="l02486"></a>02486           printf(<span class="stringliteral">&quot;\ndamping term %8g, gain ratio %8g, errors %8g / %8g = %g\n&quot;</span>, mu, dL!=0.0? dF/dL : dF/DBL_EPSILON, p_eL2/nvis, pdp_eL2/nvis, p_eL2/pdp_eL2);
<a name="l02487"></a>02487 
<a name="l02488"></a>02488         <span class="keywordflow">if</span>(dL&gt;0.0 &amp;&amp; dF&gt;0.0){ <span class="comment">/* reduction in error, increment is accepted */</span>
<a name="l02489"></a>02489           tmp=(2.0*dF/dL-1.0);
<a name="l02490"></a>02490           tmp=1.0-tmp*tmp*tmp;
<a name="l02491"></a>02491           mu=mu*( (tmp&gt;=SBA_ONE_THIRD)? tmp : SBA_ONE_THIRD );
<a name="l02492"></a>02492           nu=2;
<a name="l02493"></a>02493 
<a name="l02494"></a>02494           <span class="comment">/* the test below is equivalent to the relative reduction of the RMS reprojection error: sqrt(p_eL2)-sqrt(pdp_eL2)&lt;eps4*sqrt(p_eL2) */</span>
<a name="l02495"></a>02495           <span class="keywordflow">if</span>(pdp_eL2-2.0*sqrt(p_eL2*pdp_eL2)&lt;(eps4_sq-1.0)*p_eL2) stop=4;
<a name="l02496"></a>02496           
<a name="l02497"></a>02497           <span class="keywordflow">for</span>(i=0; i&lt;nvars; ++i) <span class="comment">/* update p&#39;s estimate */</span>
<a name="l02498"></a>02498             p[i]=pdp[i];
<a name="l02499"></a>02499 
<a name="l02500"></a>02500           <span class="keywordflow">for</span>(i=0; i&lt;nobs; ++i) <span class="comment">/* update e and ||e||_2 */</span>
<a name="l02501"></a>02501             e[i]=hx[i];
<a name="l02502"></a>02502           p_eL2=pdp_eL2;
<a name="l02503"></a>02503           <span class="keywordflow">break</span>;
<a name="l02504"></a>02504         }
<a name="l02505"></a>02505       } <span class="comment">/* nsolved==n */</span>
<a name="l02506"></a>02506 
<a name="l02507"></a>02507       <span class="comment">/* if this point is reached, either at least one linear system could not be solved or</span>
<a name="l02508"></a>02508 <span class="comment">       * the error did not reduce; in any case, the increment must be rejected</span>
<a name="l02509"></a>02509 <span class="comment">       */</span>
<a name="l02510"></a>02510 
<a name="l02511"></a>02511       mu*=nu;
<a name="l02512"></a>02512       nu2=nu&lt;&lt;1; <span class="comment">// 2*nu;</span>
<a name="l02513"></a>02513       <span class="keywordflow">if</span>(nu2&lt;=nu){ <span class="comment">/* nu has wrapped around (overflown) */</span>
<a name="l02514"></a>02514         fprintf(stderr, <span class="stringliteral">&quot;SBA: too many failed attempts to increase the damping factor in sba_str_levmar_x()! Singular Hessian matrix?\n&quot;</span>);
<a name="l02515"></a>02515         <span class="comment">//exit(1);</span>
<a name="l02516"></a>02516         stop=6;
<a name="l02517"></a>02517         <span class="keywordflow">break</span>;
<a name="l02518"></a>02518       }
<a name="l02519"></a>02519       nu=nu2;
<a name="l02520"></a>02520 
<a name="l02521"></a>02521 <span class="preprocessor">#if 0</span>
<a name="l02522"></a>02522 <span class="preprocessor"></span>      <span class="comment">/* restore V diagonal entries */</span>
<a name="l02523"></a>02523       <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l02524"></a>02524         ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l02525"></a>02525         ptr2=diagV + i*pnp; <span class="comment">// set ptr2 to point to diagV_i</span>
<a name="l02526"></a>02526         <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l02527"></a>02527           ptr1[j*pnp+j]=ptr2[j];
<a name="l02528"></a>02528       }
<a name="l02529"></a>02529 <span class="preprocessor">#endif</span>
<a name="l02530"></a>02530 <span class="preprocessor"></span>    } <span class="comment">/* inner while loop */</span>
<a name="l02531"></a>02531 
<a name="l02532"></a>02532     <span class="keywordflow">if</span>(p_eL2&lt;=eps3_sq) stop=5; <span class="comment">// error is small, force termination of outer loop</span>
<a name="l02533"></a>02533   }
<a name="l02534"></a>02534 
<a name="l02535"></a>02535   <span class="keywordflow">if</span>(itno&gt;=itmax) stop=3;
<a name="l02536"></a>02536 
<a name="l02537"></a>02537   <span class="comment">/* restore V diagonal entries */</span>
<a name="l02538"></a>02538   <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l02539"></a>02539     ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l02540"></a>02540     ptr2=diagV + i*pnp; <span class="comment">// set ptr2 to point to diagV_i</span>
<a name="l02541"></a>02541     <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l02542"></a>02542       ptr1[j*pnp+j]=ptr2[j];
<a name="l02543"></a>02543   }
<a name="l02544"></a>02544 
<a name="l02545"></a>02545   <span class="keywordflow">if</span>(info){
<a name="l02546"></a>02546     info[0]=init_p_eL2;
<a name="l02547"></a>02547     info[1]=p_eL2;
<a name="l02548"></a>02548     info[2]=eb_inf;
<a name="l02549"></a>02549     info[3]=dp_L2;
<a name="l02550"></a>02550     <span class="keywordflow">for</span>(i=ncon; i&lt;n; ++i){
<a name="l02551"></a>02551       ptr1=V + i*Vsz; <span class="comment">// set ptr1 to point to V_i</span>
<a name="l02552"></a>02552       <span class="keywordflow">for</span>(j=0; j&lt;pnp; ++j)
<a name="l02553"></a>02553         <span class="keywordflow">if</span>(tmp&lt;ptr1[j*pnp+j]) tmp=ptr1[j*pnp+j];
<a name="l02554"></a>02554       }
<a name="l02555"></a>02555     info[4]=mu/tmp;
<a name="l02556"></a>02556     info[5]=itno;
<a name="l02557"></a>02557     info[6]=stop;
<a name="l02558"></a>02558     info[7]=nfev;
<a name="l02559"></a>02559     info[8]=njev;
<a name="l02560"></a>02560     info[9]=nlss;
<a name="l02561"></a>02561   }
<a name="l02562"></a>02562   <span class="comment">//sba_print_sol(n, m, p, 0, pnp, x, mnp, &amp;idxij, rcidxs, rcsubs);</span>
<a name="l02563"></a>02563   retval=(stop!=7)?  itno : SBA_ERROR;
<a name="l02564"></a>02564                                                                
<a name="l02565"></a>02565 freemem_and_return: <span class="comment">/* NOTE: this point is also reached via a goto! */</span>
<a name="l02566"></a>02566 
<a name="l02567"></a>02567    <span class="comment">/* free whatever was allocated */</span>
<a name="l02568"></a>02568   free(jac); free(V);
<a name="l02569"></a>02569   free(e); free(eb);  
<a name="l02570"></a>02570   free(dp);               
<a name="l02571"></a>02571   free(rcidxs); free(rcsubs);
<a name="l02572"></a>02572 <span class="preprocessor">#ifndef SBA_DESTROY_COVS</span>
<a name="l02573"></a>02573 <span class="preprocessor"></span>  <span class="keywordflow">if</span>(wght) free(wght);
<a name="l02574"></a>02574 <span class="preprocessor">#else</span>
<a name="l02575"></a>02575 <span class="preprocessor"></span>  <span class="comment">/* nothing to do */</span>
<a name="l02576"></a>02576 <span class="preprocessor">#endif </span><span class="comment">/* SBA_DESTROY_COVS */</span>
<a name="l02577"></a>02577 
<a name="l02578"></a>02578   free(hx); free(diagV); free(pdp);
<a name="l02579"></a>02579   <span class="keywordflow">if</span>(fdj_data.hxx){ <span class="comment">// cleanup</span>
<a name="l02580"></a>02580     free(fdj_data.hxx);
<a name="l02581"></a>02581     free(fdj_data.func_rcidxs);
<a name="l02582"></a>02582   }
<a name="l02583"></a>02583 
<a name="l02584"></a>02584   sba_crsm_free(&amp;idxij);
<a name="l02585"></a>02585 
<a name="l02586"></a>02586   <span class="comment">/* free the memory allocated by the linear solver routine */</span>
<a name="l02587"></a>02587   <span class="keywordflow">if</span>(linsolver) (*linsolver)(NULL, NULL, NULL, 0, 0);
<a name="l02588"></a>02588 
<a name="l02589"></a>02589   <span class="keywordflow">return</span> retval;
<a name="l02590"></a>02590 }
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
